Merge git://git.kernel.org/pub/scm/linux/kernel/git/davem/net-next-2.6

* git://git.kernel.org/pub/scm/linux/kernel/git/davem/net-next-2.6: (1815 commits)
  mac80211: fix reorder buffer release
  iwmc3200wifi: Enable wimax core through module parameter
  iwmc3200wifi: Add wifi-wimax coexistence mode as a module parameter
  iwmc3200wifi: Coex table command does not expect a response
  iwmc3200wifi: Update wiwi priority table
  iwlwifi: driver version track kernel version
  iwlwifi: indicate uCode type when fail dump error/event log
  iwl3945: remove duplicated event logging code
  b43: fix two warnings
  ipw2100: fix rebooting hang with driver loaded
  cfg80211: indent regulatory messages with spaces
  iwmc3200wifi: fix NULL pointer dereference in pmkid update
  mac80211: Fix TX status reporting for injected data frames
  ath9k: enable 2GHz band only if the device supports it
  airo: Fix integer overflow warning
  rt2x00: Fix padding bug on L2PAD devices.
  WE: Fix set events not propagated
  b43legacy: avoid PPC fault during resume
  b43: avoid PPC fault during resume
  tcp: fix a timewait refcnt race
  ...

Fix up conflicts due to sysctl cleanups (dead sysctl_check code and
CTL_UNNUMBERED removed) in
	kernel/sysctl_check.c
	net/ipv4/sysctl_net_ipv4.c
	net/ipv6/addrconf.c
	net/sctp/sysctl.c

798 files changed, 74540 insertions, 36922 deletions

diff --git a/drivers/atm/ambassador.c b/drivers/atm/ambassador.c
index 66e181345b3a..8af23411743c 100644
--- a/drivers/atm/ambassador.c
+++ b/drivers/atm/ambassador.c
@@ -2351,6 +2351,7 @@ static void __init amb_check_args (void) {
 MODULE_AUTHOR(maintainer_string);
 MODULE_DESCRIPTION(description_string);
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE("atmsar11.fw");
 module_param(debug,   ushort, 0644);
 module_param(cmds,    uint, 0);
 module_param(txs,     uint, 0);
diff --git a/drivers/atm/fore200e.c b/drivers/atm/fore200e.c
index f766cc46b4c4..bc53fed89b1e 100644
--- a/drivers/atm/fore200e.c
+++ b/drivers/atm/fore200e.c
@@ -2906,8 +2906,8 @@ fore200e_proc_read(struct atm_dev *dev, loff_t* pos, char* page)
 	u32 media_index    = FORE200E_MEDIA_INDEX(fore200e->bus->read(&fore200e->cp_queues->media_type));
 	u32 oc3_index;
 
-	if ((media_index < 0) || (media_index > 4))
-	    media_index = 5;
+	if (media_index > 4)
+		media_index = 5;
 	
 	switch (fore200e->loop_mode) {
 	    case ATM_LM_NONE:    oc3_index = 0;
diff --git a/drivers/atm/he.c b/drivers/atm/he.c
index 70667033a568..e90665876c47 100644
--- a/drivers/atm/he.c
+++ b/drivers/atm/he.c
@@ -2739,7 +2739,7 @@ he_ioctl(struct atm_dev *atm_dev, unsigned int cmd, void __user *arg)
 			spin_lock_irqsave(&he_dev->global_lock, flags);
 			switch (reg.type) {
 				case HE_REGTYPE_PCI:
-					if (reg.addr < 0 || reg.addr >= HE_REGMAP_SIZE) {
+					if (reg.addr >= HE_REGMAP_SIZE) {
 						err = -EINVAL;
 						break;
 					}
diff --git a/drivers/atm/solos-pci.c b/drivers/atm/solos-pci.c
index c5f5186d62a3..51eed679a059 100644
--- a/drivers/atm/solos-pci.c
+++ b/drivers/atm/solos-pci.c
@@ -142,6 +142,9 @@ MODULE_AUTHOR("Traverse Technologies <support@traverse.com.au>");
 MODULE_DESCRIPTION("Solos PCI driver");
 MODULE_VERSION(VERSION);
 MODULE_LICENSE("GPL");
+MODULE_FIRMWARE("solos-FPGA.bin");
+MODULE_FIRMWARE("solos-Firmware.bin");
+MODULE_FIRMWARE("solos-db-FPGA.bin");
 MODULE_PARM_DESC(reset, "Reset Solos chips on startup");
 MODULE_PARM_DESC(atmdebug, "Print ATM data");
 MODULE_PARM_DESC(firmware_upgrade, "Initiate Solos firmware upgrade");
@@ -528,34 +531,37 @@ static int flash_upgrade(struct solos_card *card, int chip)
 	int numblocks = 0;
 	int offset;
 
-	if (chip == 0) {
+	switch (chip) {
+	case 0:
 		fw_name = "solos-FPGA.bin";
 		blocksize = FPGA_BLOCK;
-	} 
-	
-	if (chip == 1) {
+		break;
+	case 1:
 		fw_name = "solos-Firmware.bin";
 		blocksize = SOLOS_BLOCK;
-	}
-	
-	if (chip == 2){
+		break;
+	case 2:
 		if (card->fpga_version > LEGACY_BUFFERS){
 			fw_name = "solos-db-FPGA.bin";
 			blocksize = FPGA_BLOCK;
 		} else {
-			dev_info(&card->dev->dev, "FPGA version doesn't support daughter board upgrades\n");
+			dev_info(&card->dev->dev, "FPGA version doesn't support"
+					" daughter board upgrades\n");
 			return -EPERM;
 		}
-	}
-	
-	if (chip == 3){
+		break;
+	case 3:
 		if (card->fpga_version > LEGACY_BUFFERS){
 			fw_name = "solos-Firmware.bin";
 			blocksize = SOLOS_BLOCK;
 		} else {
-		dev_info(&card->dev->dev, "FPGA version doesn't support daughter board upgrades\n");
-		return -EPERM;
+			dev_info(&card->dev->dev, "FPGA version doesn't support"
+					" daughter board upgrades\n");
+			return -EPERM;
 		}
+		break;
+	default:
+		return -ENODEV;
 	}
 
 	if (request_firmware(&fw, fw_name, &card->dev->dev))
diff --git a/drivers/bluetooth/btmrvl_debugfs.c b/drivers/bluetooth/btmrvl_debugfs.c
index 4617bd12f63b..d43b5cb864ef 100644
--- a/drivers/bluetooth/btmrvl_debugfs.c
+++ b/drivers/bluetooth/btmrvl_debugfs.c
@@ -29,7 +29,6 @@ struct btmrvl_debugfs_data {
 	struct dentry *root_dir, *config_dir, *status_dir;
 
 	/* config */
-	struct dentry *drvdbg;
 	struct dentry *psmode;
 	struct dentry *pscmd;
 	struct dentry *hsmode;
diff --git a/drivers/bluetooth/btmrvl_drv.h b/drivers/bluetooth/btmrvl_drv.h
index 411c7a77082d..523d197b9824 100644
--- a/drivers/bluetooth/btmrvl_drv.h
+++ b/drivers/bluetooth/btmrvl_drv.h
@@ -131,6 +131,7 @@ void btmrvl_check_evtpkt(struct btmrvl_private *priv, struct sk_buff *skb);
 int btmrvl_process_event(struct btmrvl_private *priv, struct sk_buff *skb);
 
 int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd);
+int btmrvl_enable_ps(struct btmrvl_private *priv);
 int btmrvl_prepare_command(struct btmrvl_private *priv);
 
 #ifdef CONFIG_DEBUG_FS
diff --git a/drivers/bluetooth/btmrvl_main.c b/drivers/bluetooth/btmrvl_main.c
index e605563b4eaa..f97771ce432c 100644
--- a/drivers/bluetooth/btmrvl_main.c
+++ b/drivers/bluetooth/btmrvl_main.c
@@ -189,6 +189,38 @@ int btmrvl_send_module_cfg_cmd(struct btmrvl_private *priv, int subcmd)
 }
 EXPORT_SYMBOL_GPL(btmrvl_send_module_cfg_cmd);
 
+int btmrvl_enable_ps(struct btmrvl_private *priv)
+{
+	struct sk_buff *skb;
+	struct btmrvl_cmd *cmd;
+
+	skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
+	if (skb == NULL) {
+		BT_ERR("No free skb");
+		return -ENOMEM;
+	}
+
+	cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
+	cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF,
+					BT_CMD_AUTO_SLEEP_MODE));
+	cmd->length = 1;
+
+	if (priv->btmrvl_dev.psmode)
+		cmd->data[0] = BT_PS_ENABLE;
+	else
+		cmd->data[0] = BT_PS_DISABLE;
+
+	bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
+
+	skb->dev = (void *) priv->btmrvl_dev.hcidev;
+	skb_queue_head(&priv->adapter->tx_queue, skb);
+
+	BT_DBG("Queue PSMODE Command:%d", cmd->data[0]);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(btmrvl_enable_ps);
+
 static int btmrvl_enable_hs(struct btmrvl_private *priv)
 {
 	struct sk_buff *skb;
@@ -258,28 +290,7 @@ int btmrvl_prepare_command(struct btmrvl_private *priv)
 
 	if (priv->btmrvl_dev.pscmd) {
 		priv->btmrvl_dev.pscmd = 0;
-
-		skb = bt_skb_alloc(sizeof(*cmd), GFP_ATOMIC);
-		if (skb == NULL) {
-			BT_ERR("No free skb");
-			return -ENOMEM;
-		}
-
-		cmd = (struct btmrvl_cmd *) skb_put(skb, sizeof(*cmd));
-		cmd->ocf_ogf = cpu_to_le16(hci_opcode_pack(OGF, BT_CMD_AUTO_SLEEP_MODE));
-		cmd->length = 1;
-
-		if (priv->btmrvl_dev.psmode)
-			cmd->data[0] = BT_PS_ENABLE;
-		else
-			cmd->data[0] = BT_PS_DISABLE;
-
-		bt_cb(skb)->pkt_type = MRVL_VENDOR_PKT;
-
-		skb->dev = (void *) priv->btmrvl_dev.hcidev;
-		skb_queue_head(&priv->adapter->tx_queue, skb);
-
-		BT_DBG("Queue PSMODE Command:%d", cmd->data[0]);
+		btmrvl_enable_ps(priv);
 	}
 
 	if (priv->btmrvl_dev.hscmd) {
diff --git a/drivers/bluetooth/btmrvl_sdio.c b/drivers/bluetooth/btmrvl_sdio.c
index 5b33b85790f2..1e6eb1aeba2b 100644
--- a/drivers/bluetooth/btmrvl_sdio.c
+++ b/drivers/bluetooth/btmrvl_sdio.c
@@ -930,6 +930,8 @@ static int btmrvl_sdio_probe(struct sdio_func *func,
 	priv->hw_wakeup_firmware = btmrvl_sdio_wakeup_fw;
 
 	btmrvl_send_module_cfg_cmd(priv, MODULE_BRINGUP_REQ);
+	priv->btmrvl_dev.psmode = 1;
+	btmrvl_enable_ps(priv);
 
 	return 0;
 
@@ -1001,3 +1003,5 @@ MODULE_AUTHOR("Marvell International Ltd.");
 MODULE_DESCRIPTION("Marvell BT-over-SDIO driver ver " VERSION);
 MODULE_VERSION(VERSION);
 MODULE_LICENSE("GPL v2");
+MODULE_FIRMWARE("sd8688_helper.bin");
+MODULE_FIRMWARE("sd8688.bin");
diff --git a/drivers/bluetooth/hci_vhci.c b/drivers/bluetooth/hci_vhci.c
index d5cde6d86f89..7595274103fd 100644
--- a/drivers/bluetooth/hci_vhci.c
+++ b/drivers/bluetooth/hci_vhci.c
@@ -41,8 +41,6 @@
 
 #define VERSION "1.3"
 
-static int minor = MISC_DYNAMIC_MINOR;
-
 struct vhci_data {
 	struct hci_dev *hdev;
 
@@ -218,12 +216,6 @@ static unsigned int vhci_poll(struct file *file, poll_table *wait)
 	return POLLOUT | POLLWRNORM;
 }
 
-static int vhci_ioctl(struct inode *inode, struct file *file,
-					unsigned int cmd, unsigned long arg)
-{
-	return -EINVAL;
-}
-
 static int vhci_open(struct inode *inode, struct file *file)
 {
 	struct vhci_data *data;
@@ -284,10 +276,10 @@ static int vhci_release(struct inode *inode, struct file *file)
 }
 
 static const struct file_operations vhci_fops = {
+	.owner		= THIS_MODULE,
 	.read		= vhci_read,
 	.write		= vhci_write,
 	.poll		= vhci_poll,
-	.ioctl		= vhci_ioctl,
 	.open		= vhci_open,
 	.release	= vhci_release,
 };
@@ -302,18 +294,12 @@ static int __init vhci_init(void)
 {
 	BT_INFO("Virtual HCI driver ver %s", VERSION);
 
-	if (misc_register(&vhci_miscdev) < 0) {
-		BT_ERR("Can't register misc device with minor %d", minor);
-		return -EIO;
-	}
-
-	return 0;
+	return misc_register(&vhci_miscdev);
 }
 
 static void __exit vhci_exit(void)
 {
-	if (misc_deregister(&vhci_miscdev) < 0)
-		BT_ERR("Can't unregister misc device with minor %d", minor);
+	misc_deregister(&vhci_miscdev);
 }
 
 module_init(vhci_init);
diff --git a/drivers/ieee802154/fakehard.c b/drivers/ieee802154/fakehard.c
index 7c544f7c74c4..d9d0e13efe47 100644
--- a/drivers/ieee802154/fakehard.c
+++ b/drivers/ieee802154/fakehard.c
@@ -32,9 +32,29 @@
 #include <net/nl802154.h>
 #include <net/wpan-phy.h>
 
-struct wpan_phy *net_to_phy(struct net_device *dev)
+struct fakehard_priv {
+	struct wpan_phy *phy;
+};
+
+static struct wpan_phy *fake_to_phy(const struct net_device *dev)
 {
-	return container_of(dev->dev.parent, struct wpan_phy, dev);
+	struct fakehard_priv *priv = netdev_priv(dev);
+	return priv->phy;
+}
+
+/**
+ * fake_get_phy - Return a phy corresponding to this device.
+ * @dev: The network device for which to return the wan-phy object
+ *
+ * This function returns a wpan-phy object corresponding to the passed
+ * network device. Reference counter for wpan-phy object is incremented,
+ * so when the wpan-phy isn't necessary, you should drop the reference
+ * via @wpan_phy_put() call.
+ */
+static struct wpan_phy *fake_get_phy(const struct net_device *dev)
+{
+	struct wpan_phy *phy = fake_to_phy(dev);
+	return to_phy(get_device(&phy->dev));
 }
 
 /**
@@ -43,7 +63,7 @@ struct wpan_phy *net_to_phy(struct net_device *dev)
  *
  * Return the ID of the PAN from the PIB.
  */
-static u16 fake_get_pan_id(struct net_device *dev)
+static u16 fake_get_pan_id(const struct net_device *dev)
 {
 	BUG_ON(dev->type != ARPHRD_IEEE802154);
 
@@ -58,7 +78,7 @@ static u16 fake_get_pan_id(struct net_device *dev)
  * device. If the device has not yet had a short address assigned
  * then this should return 0xFFFF to indicate a lack of association.
  */
-static u16 fake_get_short_addr(struct net_device *dev)
+static u16 fake_get_short_addr(const struct net_device *dev)
 {
 	BUG_ON(dev->type != ARPHRD_IEEE802154);
 
@@ -78,7 +98,7 @@ static u16 fake_get_short_addr(struct net_device *dev)
  * Note: This is in section 7.2.1.2 of the IEEE 802.15.4-2006
  *       document.
  */
-static u8 fake_get_dsn(struct net_device *dev)
+static u8 fake_get_dsn(const struct net_device *dev)
 {
 	BUG_ON(dev->type != ARPHRD_IEEE802154);
 
@@ -98,7 +118,7 @@ static u8 fake_get_dsn(struct net_device *dev)
  * Note: This is in section 7.2.1.2 of the IEEE 802.15.4-2006
  *       document.
  */
-static u8 fake_get_bsn(struct net_device *dev)
+static u8 fake_get_bsn(const struct net_device *dev)
 {
 	BUG_ON(dev->type != ARPHRD_IEEE802154);
 
@@ -121,7 +141,7 @@ static u8 fake_get_bsn(struct net_device *dev)
 static int fake_assoc_req(struct net_device *dev,
 		struct ieee802154_addr *addr, u8 channel, u8 page, u8 cap)
 {
-	struct wpan_phy *phy = net_to_phy(dev);
+	struct wpan_phy *phy = fake_to_phy(dev);
 
 	mutex_lock(&phy->pib_lock);
 	phy->current_channel = channel;
@@ -196,7 +216,7 @@ static int fake_start_req(struct net_device *dev, struct ieee802154_addr *addr,
 				u8 bcn_ord, u8 sf_ord, u8 pan_coord, u8 blx,
 				u8 coord_realign)
 {
-	struct wpan_phy *phy = net_to_phy(dev);
+	struct wpan_phy *phy = fake_to_phy(dev);
 
 	mutex_lock(&phy->pib_lock);
 	phy->current_channel = channel;
@@ -239,6 +259,8 @@ static struct ieee802154_mlme_ops fake_mlme = {
 	.start_req = fake_start_req,
 	.scan_req = fake_scan_req,
 
+	.get_phy = fake_get_phy,
+
 	.get_pan_id = fake_get_pan_id,
 	.get_short_addr = fake_get_short_addr,
 	.get_dsn = fake_get_dsn,
@@ -310,7 +332,7 @@ static const struct net_device_ops fake_ops = {
 
 static void ieee802154_fake_destruct(struct net_device *dev)
 {
-	struct wpan_phy *phy = net_to_phy(dev);
+	struct wpan_phy *phy = fake_to_phy(dev);
 
 	wpan_phy_unregister(phy);
 	free_netdev(dev);
@@ -335,13 +357,14 @@ static void ieee802154_fake_setup(struct net_device *dev)
 static int __devinit ieee802154fake_probe(struct platform_device *pdev)
 {
 	struct net_device *dev;
+	struct fakehard_priv *priv;
 	struct wpan_phy *phy = wpan_phy_alloc(0);
 	int err;
 
 	if (!phy)
 		return -ENOMEM;
 
-	dev = alloc_netdev(0, "hardwpan%d", ieee802154_fake_setup);
+	dev = alloc_netdev(sizeof(struct fakehard_priv), "hardwpan%d", ieee802154_fake_setup);
 	if (!dev) {
 		wpan_phy_free(phy);
 		return -ENOMEM;
@@ -353,12 +376,23 @@ static int __devinit ieee802154fake_probe(struct platform_device *pdev)
 			dev->addr_len);
 	memcpy(dev->perm_addr, dev->dev_addr, dev->addr_len);
 
-	phy->channels_supported = (1 << 27) - 1;
+	/*
+	 * For now we'd like to emulate 2.4 GHz-only device,
+	 * both O-QPSK and CSS
+	 */
+	/* 2.4 GHz O-QPSK 802.15.4-2003 */
+	phy->channels_supported[0] |= 0x7FFF800;
+	/* 2.4 GHz CSS 802.15.4a-2007 */
+	phy->channels_supported[3] |= 0x3fff;
+
 	phy->transmit_power = 0xbf;
 
 	dev->netdev_ops = &fake_ops;
 	dev->ml_priv = &fake_mlme;
 
+	priv = netdev_priv(dev);
+	priv->phy = phy;
+
 	/*
 	 * If the name is a format string the caller wants us to do a
 	 * name allocation.
@@ -369,11 +403,12 @@ static int __devinit ieee802154fake_probe(struct platform_device *pdev)
 			goto out;
 	}
 
+	wpan_phy_set_dev(phy, &pdev->dev);
 	SET_NETDEV_DEV(dev, &phy->dev);
 
 	platform_set_drvdata(pdev, dev);
 
-	err = wpan_phy_register(&pdev->dev, phy);
+	err = wpan_phy_register(phy);
 	if (err)
 		goto out;
 
diff --git a/drivers/infiniband/hw/nes/nes_nic.c b/drivers/infiniband/hw/nes/nes_nic.c
index e593af3354b8..de18fdfdadf2 100644
--- a/drivers/infiniband/hw/nes/nes_nic.c
+++ b/drivers/infiniband/hw/nes/nes_nic.c
@@ -1080,11 +1080,14 @@ static int nes_netdev_set_rx_csum(struct net_device *netdev, u32 enable)
 
 
 /**
- * nes_netdev_get_stats_count
+ * nes_netdev_get_sset_count
  */
-static int nes_netdev_get_stats_count(struct net_device *netdev)
+static int nes_netdev_get_sset_count(struct net_device *netdev, int stringset)
 {
-	return NES_ETHTOOL_STAT_COUNT;
+	if (stringset == ETH_SS_STATS)
+		return NES_ETHTOOL_STAT_COUNT;
+	else
+		return -EINVAL;
 }
 
 
@@ -1264,7 +1267,6 @@ static void nes_netdev_get_drvinfo(struct net_device *netdev,
 	sprintf(drvinfo->fw_version, "%u.%u", nesadapter->firmware_version>>16,
 				nesadapter->firmware_version & 0x000000ff);
 	strcpy(drvinfo->version, DRV_VERSION);
-	drvinfo->n_stats = nes_netdev_get_stats_count(netdev);
 	drvinfo->testinfo_len = 0;
 	drvinfo->eedump_len = 0;
 	drvinfo->regdump_len = 0;
@@ -1516,7 +1518,7 @@ static const struct ethtool_ops nes_ethtool_ops = {
 	.get_rx_csum = nes_netdev_get_rx_csum,
 	.get_sg = ethtool_op_get_sg,
 	.get_strings = nes_netdev_get_strings,
-	.get_stats_count = nes_netdev_get_stats_count,
+	.get_sset_count = nes_netdev_get_sset_count,
 	.get_ethtool_stats = nes_netdev_get_ethtool_stats,
 	.get_drvinfo = nes_netdev_get_drvinfo,
 	.get_coalesce = nes_netdev_get_coalesce,
diff --git a/drivers/isdn/gigaset/Kconfig b/drivers/isdn/gigaset/Kconfig
index 18ab8652aa57..dcefedc7044a 100644
--- a/drivers/isdn/gigaset/Kconfig
+++ b/drivers/isdn/gigaset/Kconfig
@@ -1,6 +1,5 @@
 menuconfig ISDN_DRV_GIGASET
 	tristate "Siemens Gigaset support"
-	depends on ISDN_I4L
 	select CRC_CCITT
 	select BITREVERSE
 	help
@@ -11,9 +10,33 @@ menuconfig ISDN_DRV_GIGASET
 	  If you have one of these devices, say M here and for at least
 	  one of the connection specific parts that follow.
 	  This will build a module called "gigaset".
+	  Note: If you build your ISDN subsystem (ISDN_CAPI or ISDN_I4L)
+	  as a module, you have to build this driver as a module too,
+	  otherwise the Gigaset device won't show up as an ISDN device.
 
 if ISDN_DRV_GIGASET
 
+config GIGASET_CAPI
+	bool "Gigaset CAPI support (EXPERIMENTAL)"
+	depends on EXPERIMENTAL
+	depends on ISDN_CAPI='y'||(ISDN_CAPI='m'&&ISDN_DRV_GIGASET='m')
+	default ISDN_I4L='n'
+	help
+	  Build the Gigaset driver as a CAPI 2.0 driver interfacing with
+	  the Kernel CAPI subsystem. To use it with the old ISDN4Linux
+	  subsystem you'll have to enable the capidrv glue driver.
+	  (select ISDN_CAPI_CAPIDRV.)
+	  Say N to build the old native ISDN4Linux variant.
+
+config GIGASET_I4L
+	bool
+	depends on ISDN_I4L='y'||(ISDN_I4L='m'&&ISDN_DRV_GIGASET='m')
+	default !GIGASET_CAPI
+
+config GIGASET_DUMMYLL
+	bool
+	default !GIGASET_CAPI&&!GIGASET_I4L
+
 config GIGASET_BASE
 	tristate "Gigaset base station support"
 	depends on USB
diff --git a/drivers/isdn/gigaset/Makefile b/drivers/isdn/gigaset/Makefile
index e9d3189f56b7..c453b72272a0 100644
--- a/drivers/isdn/gigaset/Makefile
+++ b/drivers/isdn/gigaset/Makefile
@@ -1,4 +1,7 @@
-gigaset-y := common.o interface.o proc.o ev-layer.o i4l.o asyncdata.o
+gigaset-y := common.o interface.o proc.o ev-layer.o asyncdata.o
+gigaset-$(CONFIG_GIGASET_CAPI) += capi.o
+gigaset-$(CONFIG_GIGASET_I4L) += i4l.o
+gigaset-$(CONFIG_GIGASET_DUMMYLL) += dummyll.o
 usb_gigaset-y := usb-gigaset.o
 ser_gigaset-y := ser-gigaset.o
 bas_gigaset-y := bas-gigaset.o isocdata.o
diff --git a/drivers/isdn/gigaset/asyncdata.c b/drivers/isdn/gigaset/asyncdata.c
index 44a58e6f8f65..ccb2a7b7c41d 100644
--- a/drivers/isdn/gigaset/asyncdata.c
+++ b/drivers/isdn/gigaset/asyncdata.c
@@ -19,7 +19,7 @@
 
 /* check if byte must be stuffed/escaped
  * I'm not sure which data should be encoded.
- * Therefore I will go the hard way and decode every value
+ * Therefore I will go the hard way and encode every value
  * less than 0x20, the flag sequence and the control escape char.
  */
 static inline int muststuff(unsigned char c)
@@ -35,303 +35,383 @@ static inline int muststuff(unsigned char c)
 
 /* == data input =========================================================== */
 
-/* process a block of received bytes in command mode (modem response)
+/* process a block of received bytes in command mode
+ * (mstate != MS_LOCKED && (inputstate & INS_command))
+ * Append received bytes to the command response buffer and forward them
+ * line by line to the response handler. Exit whenever a mode/state change
+ * might have occurred.
  * Return value:
  *	number of processed bytes
  */
-static inline int cmd_loop(unsigned char c, unsigned char *src, int numbytes,
-			   struct inbuf_t *inbuf)
+static unsigned cmd_loop(unsigned numbytes, struct inbuf_t *inbuf)
 {
+	unsigned char *src = inbuf->data + inbuf->head;
 	struct cardstate *cs = inbuf->cs;
-	unsigned cbytes      = cs->cbytes;
-	int inputstate = inbuf->inputstate;
-	int startbytes = numbytes;
-
-	for (;;) {
-		cs->respdata[cbytes] = c;
-		if (c == 10 || c == 13) {
-			gig_dbg(DEBUG_TRANSCMD, "%s: End of Command (%d Bytes)",
+	unsigned cbytes = cs->cbytes;
+	unsigned procbytes = 0;
+	unsigned char c;
+
+	while (procbytes < numbytes) {
+		c = *src++;
+		procbytes++;
+
+		switch (c) {
+		case '\n':
+			if (cbytes == 0 && cs->respdata[0] == '\r') {
+				/* collapse LF with preceding CR */
+				cs->respdata[0] = 0;
+				break;
+			}
+			/* --v-- fall through --v-- */
+		case '\r':
+			/* end of message line, pass to response handler */
+			gig_dbg(DEBUG_TRANSCMD, "%s: End of Message (%d Bytes)",
 				__func__, cbytes);
+			if (cbytes >= MAX_RESP_SIZE) {
+				dev_warn(cs->dev, "response too large (%d)\n",
+					 cbytes);
+				cbytes = MAX_RESP_SIZE;
+			}
 			cs->cbytes = cbytes;
-			gigaset_handle_modem_response(cs); /* can change
-							      cs->dle */
+			gigaset_handle_modem_response(cs);
 			cbytes = 0;
 
-			if (cs->dle &&
-			    !(inputstate & INS_DLE_command)) {
-				inputstate &= ~INS_command;
-				break;
-			}
-		} else {
-			/* advance in line buffer, checking for overflow */
-			if (cbytes < MAX_RESP_SIZE - 1)
-				cbytes++;
-			else
-				dev_warn(cs->dev, "response too large\n");
-		}
+			/* store EOL byte for CRLF collapsing */
+			cs->respdata[0] = c;
 
-		if (!numbytes)
-			break;
-		c = *src++;
-		--numbytes;
-		if (c == DLE_FLAG &&
-		    (cs->dle || inputstate & INS_DLE_command)) {
-			inputstate |= INS_DLE_char;
-			break;
+			/* cs->dle may have changed */
+			if (cs->dle && !(inbuf->inputstate & INS_DLE_command))
+				inbuf->inputstate &= ~INS_command;
+
+			/* return for reevaluating state */
+			goto exit;
+
+		case DLE_FLAG:
+			if (inbuf->inputstate & INS_DLE_char) {
+				/* quoted DLE: clear quote flag */
+				inbuf->inputstate &= ~INS_DLE_char;
+			} else if (cs->dle ||
+				   (inbuf->inputstate & INS_DLE_command)) {
+				/* DLE escape, pass up for handling */
+				inbuf->inputstate |= INS_DLE_char;
+				goto exit;
+			}
+			/* quoted or not in DLE mode: treat as regular data */
+			/* --v-- fall through --v-- */
+		default:
+			/* append to line buffer if possible */
+			if (cbytes < MAX_RESP_SIZE)
+				cs->respdata[cbytes] = c;
+			cbytes++;
 		}
 	}
-
+exit:
 	cs->cbytes = cbytes;
-	inbuf->inputstate = inputstate;
-
-	return startbytes - numbytes;
+	return procbytes;
 }
 
-/* process a block of received bytes in lock mode (tty i/f)
+/* process a block of received bytes in lock mode
+ * All received bytes are passed unmodified to the tty i/f.
  * Return value:
  *	number of processed bytes
  */
-static inline int lock_loop(unsigned char *src, int numbytes,
-			    struct inbuf_t *inbuf)
+static unsigned lock_loop(unsigned numbytes, struct inbuf_t *inbuf)
 {
-	struct cardstate *cs = inbuf->cs;
-
-	gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response",
-			   numbytes, src);
-	gigaset_if_receive(cs, src, numbytes);
+	unsigned char *src = inbuf->data + inbuf->head;
 
+	gigaset_dbg_buffer(DEBUG_LOCKCMD, "received response", numbytes, src);
+	gigaset_if_receive(inbuf->cs, src, numbytes);
 	return numbytes;
 }
 
+/* set up next receive skb for data mode
+ */
+static void new_rcv_skb(struct bc_state *bcs)
+{
+	struct cardstate *cs = bcs->cs;
+	unsigned short hw_hdr_len = cs->hw_hdr_len;
+
+	if (bcs->ignore) {
+		bcs->skb = NULL;
+		return;
+	}
+
+	bcs->skb = dev_alloc_skb(SBUFSIZE + hw_hdr_len);
+	if (bcs->skb == NULL) {
+		dev_warn(cs->dev, "could not allocate new skb\n");
+		return;
+	}
+	skb_reserve(bcs->skb, hw_hdr_len);
+}
+
 /* process a block of received bytes in HDLC data mode
+ * (mstate != MS_LOCKED && !(inputstate & INS_command) && proto2 == L2_HDLC)
  * Collect HDLC frames, undoing byte stuffing and watching for DLE escapes.
  * When a frame is complete, check the FCS and pass valid frames to the LL.
  * If DLE is encountered, return immediately to let the caller handle it.
  * Return value:
  *	number of processed bytes
- *	numbytes (all bytes processed) on error --FIXME
  */
-static inline int hdlc_loop(unsigned char c, unsigned char *src, int numbytes,
-			    struct inbuf_t *inbuf)
+static unsigned hdlc_loop(unsigned numbytes, struct inbuf_t *inbuf)
 {
 	struct cardstate *cs = inbuf->cs;
-	struct bc_state *bcs = inbuf->bcs;
+	struct bc_state *bcs = cs->bcs;
 	int inputstate = bcs->inputstate;
 	__u16 fcs = bcs->fcs;
 	struct sk_buff *skb = bcs->skb;
-	unsigned char error;
-	struct sk_buff *compskb;
-	int startbytes = numbytes;
-	int l;
+	unsigned char *src = inbuf->data + inbuf->head;
+	unsigned procbytes = 0;
+	unsigned char c;
 
-	if (unlikely(inputstate & INS_byte_stuff)) {
+	if (inputstate & INS_byte_stuff) {
+		if (!numbytes)
+			return 0;
 		inputstate &= ~INS_byte_stuff;
 		goto byte_stuff;
 	}
-	for (;;) {
-		if (unlikely(c == PPP_ESCAPE)) {
-			if (unlikely(!numbytes)) {
-				inputstate |= INS_byte_stuff;
+
+	while (procbytes < numbytes) {
+		c = *src++;
+		procbytes++;
+		if (c == DLE_FLAG) {
+			if (inputstate & INS_DLE_char) {
+				/* quoted DLE: clear quote flag */
+				inputstate &= ~INS_DLE_char;
+			} else if (cs->dle || (inputstate & INS_DLE_command)) {
+				/* DLE escape, pass up for handling */
+				inputstate |= INS_DLE_char;
 				break;
 			}
-			c = *src++;
-			--numbytes;
-			if (unlikely(c == DLE_FLAG &&
-				     (cs->dle ||
-				      inbuf->inputstate & INS_DLE_command))) {
-				inbuf->inputstate |= INS_DLE_char;
+		}
+
+		if (c == PPP_ESCAPE) {
+			/* byte stuffing indicator: pull in next byte */
+			if (procbytes >= numbytes) {
+				/* end of buffer, save for later processing */
 				inputstate |= INS_byte_stuff;
 				break;
 			}
 byte_stuff:
+			c = *src++;
+			procbytes++;
+			if (c == DLE_FLAG) {
+				if (inputstate & INS_DLE_char) {
+					/* quoted DLE: clear quote flag */
+					inputstate &= ~INS_DLE_char;
+				} else if (cs->dle ||
+					   (inputstate & INS_DLE_command)) {
+					/* DLE escape, pass up for handling */
+					inputstate |=
+						INS_DLE_char | INS_byte_stuff;
+					break;
+				}
+			}
 			c ^= PPP_TRANS;
-			if (unlikely(!muststuff(c)))
-				gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
-		} else if (unlikely(c == PPP_FLAG)) {
-			if (unlikely(inputstate & INS_skip_frame)) {
-#ifdef CONFIG_GIGASET_DEBUG
-				if (!(inputstate & INS_have_data)) { /* 7E 7E */
-					++bcs->emptycount;
-				} else
-					gig_dbg(DEBUG_HDLC,
-					    "7e----------------------------");
-#endif
-
-				/* end of frame */
-				error = 1;
-				gigaset_rcv_error(NULL, cs, bcs);
-			} else if (!(inputstate & INS_have_data)) { /* 7E 7E */
 #ifdef CONFIG_GIGASET_DEBUG
-				++bcs->emptycount;
+			if (!muststuff(c))
+				gig_dbg(DEBUG_HDLC, "byte stuffed: 0x%02x", c);
 #endif
-				break;
-			} else {
+		} else if (c == PPP_FLAG) {
+			/* end of frame: process content if any */
+			if (inputstate & INS_have_data) {
 				gig_dbg(DEBUG_HDLC,
 					"7e----------------------------");
 
-				/* end of frame */
-				error = 0;
-
-				if (unlikely(fcs != PPP_GOODFCS)) {
+				/* check and pass received frame */
+				if (!skb) {
+					/* skipped frame */
+					gigaset_isdn_rcv_err(bcs);
+				} else if (skb->len < 2) {
+					/* frame too short for FCS */
+					dev_warn(cs->dev,
+						 "short frame (%d)\n",
+						 skb->len);
+					gigaset_isdn_rcv_err(bcs);
+					dev_kfree_skb_any(skb);
+				} else if (fcs != PPP_GOODFCS) {
+					/* frame check error */
 					dev_err(cs->dev,
 				"Checksum failed, %u bytes corrupted!\n",
 						skb->len);
-					compskb = NULL;
-					gigaset_rcv_error(compskb, cs, bcs);
-					error = 1;
+					gigaset_isdn_rcv_err(bcs);
+					dev_kfree_skb_any(skb);
 				} else {
-					if (likely((l = skb->len) > 2)) {
-						skb->tail -= 2;
-						skb->len -= 2;
-					} else {
-						dev_kfree_skb(skb);
-						skb = NULL;
-						inputstate |= INS_skip_frame;
-						if (l == 1) {
-							dev_err(cs->dev,
-						  "invalid packet size (1)!\n");
-							error = 1;
-							gigaset_rcv_error(NULL,
-								cs, bcs);
-						}
-					}
-					if (likely(!(error ||
-						     (inputstate &
-						      INS_skip_frame)))) {
-						gigaset_rcv_skb(skb, cs, bcs);
-					}
+					/* good frame */
+					__skb_trim(skb, skb->len - 2);
+					gigaset_skb_rcvd(bcs, skb);
 				}
-			}
 
-			if (unlikely(error))
-				if (skb)
-					dev_kfree_skb(skb);
-
-			fcs = PPP_INITFCS;
-			inputstate &= ~(INS_have_data | INS_skip_frame);
-			if (unlikely(bcs->ignore)) {
-				inputstate |= INS_skip_frame;
-				skb = NULL;
-			} else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)) {
-				skb_reserve(skb, HW_HDR_LEN);
+				/* prepare reception of next frame */
+				inputstate &= ~INS_have_data;
+				new_rcv_skb(bcs);
+				skb = bcs->skb;
 			} else {
-				dev_warn(cs->dev,
-					 "could not allocate new skb\n");
-				inputstate |= INS_skip_frame;
+				/* empty frame (7E 7E) */
+#ifdef CONFIG_GIGASET_DEBUG
+				++bcs->emptycount;
+#endif
+				if (!skb) {
+					/* skipped (?) */
+					gigaset_isdn_rcv_err(bcs);
+					new_rcv_skb(bcs);
+					skb = bcs->skb;
+				}
 			}
 
-			break;
-		} else if (unlikely(muststuff(c))) {
+			fcs = PPP_INITFCS;
+			continue;
+#ifdef CONFIG_GIGASET_DEBUG
+		} else if (muststuff(c)) {
 			/* Should not happen. Possible after ZDLE=1<CR><LF>. */
 			gig_dbg(DEBUG_HDLC, "not byte stuffed: 0x%02x", c);
+#endif
 		}
 
-		/* add character */
-
+		/* regular data byte, append to skb */
 #ifdef CONFIG_GIGASET_DEBUG
-		if (unlikely(!(inputstate & INS_have_data))) {
+		if (!(inputstate & INS_have_data)) {
 			gig_dbg(DEBUG_HDLC, "7e (%d x) ================",
 				bcs->emptycount);
 			bcs->emptycount = 0;
 		}
 #endif
-
 		inputstate |= INS_have_data;
-
-		if (likely(!(inputstate & INS_skip_frame))) {
-			if (unlikely(skb->len == SBUFSIZE)) {
+		if (skb) {
+			if (skb->len == SBUFSIZE) {
 				dev_warn(cs->dev, "received packet too long\n");
 				dev_kfree_skb_any(skb);
-				skb = NULL;
-				inputstate |= INS_skip_frame;
-				break;
+				/* skip remainder of packet */
+				bcs->skb = skb = NULL;
+			} else {
+				*__skb_put(skb, 1) = c;
+				fcs = crc_ccitt_byte(fcs, c);
 			}
-			*__skb_put(skb, 1) = c;
-			fcs = crc_ccitt_byte(fcs, c);
-		}
-
-		if (unlikely(!numbytes))
-			break;
-		c = *src++;
-		--numbytes;
-		if (unlikely(c == DLE_FLAG &&
-			     (cs->dle ||
-			      inbuf->inputstate & INS_DLE_command))) {
-			inbuf->inputstate |= INS_DLE_char;
-			break;
 		}
 	}
+
 	bcs->inputstate = inputstate;
 	bcs->fcs = fcs;
-	bcs->skb = skb;
-	return startbytes - numbytes;
+	return procbytes;
 }
 
 /* process a block of received bytes in transparent data mode
+ * (mstate != MS_LOCKED && !(inputstate & INS_command) && proto2 != L2_HDLC)
  * Invert bytes, undoing byte stuffing and watching for DLE escapes.
  * If DLE is encountered, return immediately to let the caller handle it.
  * Return value:
  *	number of processed bytes
- *	numbytes (all bytes processed) on error --FIXME
  */
-static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes,
-			    struct inbuf_t *inbuf)
+static unsigned iraw_loop(unsigned numbytes, struct inbuf_t *inbuf)
 {
 	struct cardstate *cs = inbuf->cs;
-	struct bc_state *bcs = inbuf->bcs;
+	struct bc_state *bcs = cs->bcs;
 	int inputstate = bcs->inputstate;
 	struct sk_buff *skb = bcs->skb;
-	int startbytes = numbytes;
+	unsigned char *src = inbuf->data + inbuf->head;
+	unsigned procbytes = 0;
+	unsigned char c;
 
-	for (;;) {
-		/* add character */
-		inputstate |= INS_have_data;
+	if (!skb) {
+		/* skip this block */
+		new_rcv_skb(bcs);
+		return numbytes;
+	}
 
-		if (likely(!(inputstate & INS_skip_frame))) {
-			if (unlikely(skb->len == SBUFSIZE)) {
-				//FIXME just pass skb up and allocate a new one
-				dev_warn(cs->dev, "received packet too long\n");
-				dev_kfree_skb_any(skb);
-				skb = NULL;
-				inputstate |= INS_skip_frame;
+	while (procbytes < numbytes && skb->len < SBUFSIZE) {
+		c = *src++;
+		procbytes++;
+
+		if (c == DLE_FLAG) {
+			if (inputstate & INS_DLE_char) {
+				/* quoted DLE: clear quote flag */
+				inputstate &= ~INS_DLE_char;
+			} else if (cs->dle || (inputstate & INS_DLE_command)) {
+				/* DLE escape, pass up for handling */
+				inputstate |= INS_DLE_char;
 				break;
 			}
-			*__skb_put(skb, 1) = bitrev8(c);
 		}
 
-		if (unlikely(!numbytes))
-			break;
-		c = *src++;
-		--numbytes;
-		if (unlikely(c == DLE_FLAG &&
-			     (cs->dle ||
-			      inbuf->inputstate & INS_DLE_command))) {
-			inbuf->inputstate |= INS_DLE_char;
-			break;
-		}
+		/* regular data byte: append to current skb */
+		inputstate |= INS_have_data;
+		*__skb_put(skb, 1) = bitrev8(c);
 	}
 
 	/* pass data up */
-	if (likely(inputstate & INS_have_data)) {
-		if (likely(!(inputstate & INS_skip_frame))) {
-			gigaset_rcv_skb(skb, cs, bcs);
-		}
-		inputstate &= ~(INS_have_data | INS_skip_frame);
-		if (unlikely(bcs->ignore)) {
-			inputstate |= INS_skip_frame;
-			skb = NULL;
-		} else if (likely((skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN))
-				  != NULL)) {
-			skb_reserve(skb, HW_HDR_LEN);
+	if (inputstate & INS_have_data) {
+		gigaset_skb_rcvd(bcs, skb);
+		inputstate &= ~INS_have_data;
+		new_rcv_skb(bcs);
+	}
+
+	bcs->inputstate = inputstate;
+	return procbytes;
+}
+
+/* process DLE escapes
+ * Called whenever a DLE sequence might be encountered in the input stream.
+ * Either processes the entire DLE sequence or, if that isn't possible,
+ * notes the fact that an initial DLE has been received in the INS_DLE_char
+ * inputstate flag and resumes processing of the sequence on the next call.
+ */
+static void handle_dle(struct inbuf_t *inbuf)
+{
+	struct cardstate *cs = inbuf->cs;
+
+	if (cs->mstate == MS_LOCKED)
+		return;		/* no DLE processing in lock mode */
+
+	if (!(inbuf->inputstate & INS_DLE_char)) {
+		/* no DLE pending */
+		if (inbuf->data[inbuf->head] == DLE_FLAG &&
+		    (cs->dle || inbuf->inputstate & INS_DLE_command)) {
+			/* start of DLE sequence */
+			inbuf->head++;
+			if (inbuf->head == inbuf->tail ||
+			    inbuf->head == RBUFSIZE) {
+				/* end of buffer, save for later processing */
+				inbuf->inputstate |= INS_DLE_char;
+				return;
+			}
 		} else {
-			dev_warn(cs->dev, "could not allocate new skb\n");
-			inputstate |= INS_skip_frame;
+			/* regular data byte */
+			return;
 		}
 	}
 
-	bcs->inputstate = inputstate;
-	bcs->skb = skb;
-	return startbytes - numbytes;
+	/* consume pending DLE */
+	inbuf->inputstate &= ~INS_DLE_char;
+
+	switch (inbuf->data[inbuf->head]) {
+	case 'X':	/* begin of event message */
+		if (inbuf->inputstate & INS_command)
+			dev_notice(cs->dev,
+				   "received <DLE>X in command mode\n");
+		inbuf->inputstate |= INS_command | INS_DLE_command;
+		inbuf->head++;	/* byte consumed */
+		break;
+	case '.':	/* end of event message */
+		if (!(inbuf->inputstate & INS_DLE_command))
+			dev_notice(cs->dev,
+				   "received <DLE>. without <DLE>X\n");
+		inbuf->inputstate &= ~INS_DLE_command;
+		/* return to data mode if in DLE mode */
+		if (cs->dle)
+			inbuf->inputstate &= ~INS_command;
+		inbuf->head++;	/* byte consumed */
+		break;
+	case DLE_FLAG:	/* DLE in data stream */
+		/* mark as quoted */
+		inbuf->inputstate |= INS_DLE_char;
+		if (!(cs->dle || inbuf->inputstate & INS_DLE_command))
+			dev_notice(cs->dev,
+				   "received <DLE><DLE> not in DLE mode\n");
+		break;	/* quoted byte left in buffer */
+	default:
+		dev_notice(cs->dev, "received <DLE><%02x>\n",
+			   inbuf->data[inbuf->head]);
+		/* quoted byte left in buffer */
+	}
 }
 
 /**
@@ -345,94 +425,39 @@ static inline int iraw_loop(unsigned char c, unsigned char *src, int numbytes,
  */
 void gigaset_m10x_input(struct inbuf_t *inbuf)
 {
-	struct cardstate *cs;
-	unsigned tail, head, numbytes;
-	unsigned char *src, c;
-	int procbytes;
-
-	head = inbuf->head;
-	tail = inbuf->tail;
-	gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", head, tail);
-
-	if (head != tail) {
-		cs = inbuf->cs;
-		src = inbuf->data + head;
-		numbytes = (head > tail ? RBUFSIZE : tail) - head;
-		gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
+	struct cardstate *cs = inbuf->cs;
+	unsigned numbytes, procbytes;
 
-		while (numbytes) {
-			if (cs->mstate == MS_LOCKED) {
-				procbytes = lock_loop(src, numbytes, inbuf);
-				src += procbytes;
-				numbytes -= procbytes;
-			} else {
-				c = *src++;
-				--numbytes;
-				if (c == DLE_FLAG && (cs->dle ||
-				    inbuf->inputstate & INS_DLE_command)) {
-					if (!(inbuf->inputstate & INS_DLE_char)) {
-						inbuf->inputstate |= INS_DLE_char;
-						goto nextbyte;
-					}
-					/* <DLE> <DLE> => <DLE> in data stream */
-					inbuf->inputstate &= ~INS_DLE_char;
-				}
+	gig_dbg(DEBUG_INTR, "buffer state: %u -> %u", inbuf->head, inbuf->tail);
 
-				if (!(inbuf->inputstate & INS_DLE_char)) {
-
-					/* FIXME use function pointers?  */
-					if (inbuf->inputstate & INS_command)
-						procbytes = cmd_loop(c, src, numbytes, inbuf);
-					else if (inbuf->bcs->proto2 == ISDN_PROTO_L2_HDLC)
-						procbytes = hdlc_loop(c, src, numbytes, inbuf);
-					else
-						procbytes = iraw_loop(c, src, numbytes, inbuf);
-
-					src += procbytes;
-					numbytes -= procbytes;
-				} else {  /* DLE char */
-					inbuf->inputstate &= ~INS_DLE_char;
-					switch (c) {
-					case 'X': /*begin of command*/
-						if (inbuf->inputstate & INS_command)
-							dev_warn(cs->dev,
-					"received <DLE> 'X' in command mode\n");
-						inbuf->inputstate |=
-							INS_command | INS_DLE_command;
-						break;
-					case '.': /*end of command*/
-						if (!(inbuf->inputstate & INS_command))
-							dev_warn(cs->dev,
-					"received <DLE> '.' in hdlc mode\n");
-						inbuf->inputstate &= cs->dle ?
-							~(INS_DLE_command|INS_command)
-							: ~INS_DLE_command;
-						break;
-					//case DLE_FLAG: /*DLE_FLAG in data stream*/ /* schon oben behandelt! */
-					default:
-						dev_err(cs->dev,
-						      "received 0x10 0x%02x!\n",
-							(int) c);
-						/* FIXME: reset driver?? */
-					}
-				}
-			}
-nextbyte:
-			if (!numbytes) {
-				/* end of buffer, check for wrap */
-				if (head > tail) {
-					head = 0;
-					src = inbuf->data;
-					numbytes = tail;
-				} else {
-					head = tail;
-					break;
-				}
-			}
-		}
+	while (inbuf->head != inbuf->tail) {
+		/* check for DLE escape */
+		handle_dle(inbuf);
 
-		gig_dbg(DEBUG_INTR, "setting head to %u", head);
-		inbuf->head = head;
+		/* process a contiguous block of bytes */
+		numbytes = (inbuf->head > inbuf->tail ?
+			    RBUFSIZE : inbuf->tail) - inbuf->head;
+		gig_dbg(DEBUG_INTR, "processing %u bytes", numbytes);
+		/*
+		 * numbytes may be 0 if handle_dle() ate the last byte.
+		 * This does no harm, *_loop() will just return 0 immediately.
+		 */
+
+		if (cs->mstate == MS_LOCKED)
+			procbytes = lock_loop(numbytes, inbuf);
+		else if (inbuf->inputstate & INS_command)
+			procbytes = cmd_loop(numbytes, inbuf);
+		else if (cs->bcs->proto2 == L2_HDLC)
+			procbytes = hdlc_loop(numbytes, inbuf);
+		else
+			procbytes = iraw_loop(numbytes, inbuf);
+		inbuf->head += procbytes;
+
+		/* check for buffer wraparound */
+		if (inbuf->head >= RBUFSIZE)
+			inbuf->head = 0;
+
+		gig_dbg(DEBUG_INTR, "head set to %u", inbuf->head);
 	}
 }
 EXPORT_SYMBOL_GPL(gigaset_m10x_input);
@@ -440,16 +465,16 @@ EXPORT_SYMBOL_GPL(gigaset_m10x_input);
 
 /* == data output ========================================================== */
 
-/* Encoding of a PPP packet into an octet stuffed HDLC frame
- * with FCS, opening and closing flags.
+/*
+ * Encode a data packet into an octet stuffed HDLC frame with FCS,
+ * opening and closing flags, preserving headroom data.
  * parameters:
- *	skb	skb containing original packet (freed upon return)
- *	head	number of headroom bytes to allocate in result skb
- *	tail	number of tailroom bytes to allocate in result skb
+ *	skb		skb containing original packet (freed upon return)
  * Return value:
  *	pointer to newly allocated skb containing the result frame
+ *	and the original link layer header, NULL on error
  */
-static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail)
+static struct sk_buff *HDLC_Encode(struct sk_buff *skb)
 {
 	struct sk_buff *hdlc_skb;
 	__u16 fcs;
@@ -471,16 +496,19 @@ static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail)
 
 	/* size of new buffer: original size + number of stuffing bytes
 	 * + 2 bytes FCS + 2 stuffing bytes for FCS (if needed) + 2 flag bytes
+	 * + room for link layer header
 	 */
-	hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + tail + head);
+	hdlc_skb = dev_alloc_skb(skb->len + stuf_cnt + 6 + skb->mac_len);
 	if (!hdlc_skb) {
-		dev_kfree_skb(skb);
+		dev_kfree_skb_any(skb);
 		return NULL;
 	}
-	skb_reserve(hdlc_skb, head);
 
-	/* Copy acknowledge request into new skb */
-	memcpy(hdlc_skb->head, skb->head, 2);
+	/* Copy link layer header into new skb */
+	skb_reset_mac_header(hdlc_skb);
+	skb_reserve(hdlc_skb, skb->mac_len);
+	memcpy(skb_mac_header(hdlc_skb), skb_mac_header(skb), skb->mac_len);
+	hdlc_skb->mac_len = skb->mac_len;
 
 	/* Add flag sequence in front of everything.. */
 	*(skb_put(hdlc_skb, 1)) = PPP_FLAG;
@@ -511,33 +539,42 @@ static struct sk_buff *HDLC_Encode(struct sk_buff *skb, int head, int tail)
 
 	*(skb_put(hdlc_skb, 1)) = PPP_FLAG;
 
-	dev_kfree_skb(skb);
+	dev_kfree_skb_any(skb);
 	return hdlc_skb;
 }
 
-/* Encoding of a raw packet into an octet stuffed bit inverted frame
+/*
+ * Encode a data packet into an octet stuffed raw bit inverted frame,
+ * preserving headroom data.
  * parameters:
- *	skb	skb containing original packet (freed upon return)
- *	head	number of headroom bytes to allocate in result skb
- *	tail	number of tailroom bytes to allocate in result skb
+ *	skb		skb containing original packet (freed upon return)
  * Return value:
  *	pointer to newly allocated skb containing the result frame
+ *	and the original link layer header, NULL on error
  */
-static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
+static struct sk_buff *iraw_encode(struct sk_buff *skb)
 {
 	struct sk_buff *iraw_skb;
 	unsigned char c;
 	unsigned char *cp;
 	int len;
 
-	/* worst case: every byte must be stuffed */
-	iraw_skb = dev_alloc_skb(2*skb->len + tail + head);
+	/* size of new buffer (worst case = every byte must be stuffed):
+	 * 2 * original size + room for link layer header
+	 */
+	iraw_skb = dev_alloc_skb(2*skb->len + skb->mac_len);
 	if (!iraw_skb) {
-		dev_kfree_skb(skb);
+		dev_kfree_skb_any(skb);
 		return NULL;
 	}
-	skb_reserve(iraw_skb, head);
 
+	/* copy link layer header into new skb */
+	skb_reset_mac_header(iraw_skb);
+	skb_reserve(iraw_skb, skb->mac_len);
+	memcpy(skb_mac_header(iraw_skb), skb_mac_header(skb), skb->mac_len);
+	iraw_skb->mac_len = skb->mac_len;
+
+	/* copy and stuff data */
 	cp = skb->data;
 	len = skb->len;
 	while (len--) {
@@ -546,7 +583,7 @@ static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
 			*(skb_put(iraw_skb, 1)) = c;
 		*(skb_put(iraw_skb, 1)) = c;
 	}
-	dev_kfree_skb(skb);
+	dev_kfree_skb_any(skb);
 	return iraw_skb;
 }
 
@@ -555,8 +592,10 @@ static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
  * @bcs:	B channel descriptor structure.
  * @skb:	data to send.
  *
- * Called by i4l.c to encode and queue an skb for sending, and start
+ * Called by LL to encode and queue an skb for sending, and start
  * transmission if necessary.
+ * Once the payload data has been transmitted completely, gigaset_skb_sent()
+ * will be called with the skb's link layer header preserved.
  *
  * Return value:
  *	number of bytes accepted for sending (skb->len) if ok,
@@ -564,24 +603,25 @@ static struct sk_buff *iraw_encode(struct sk_buff *skb, int head, int tail)
  */
 int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb)
 {
+	struct cardstate *cs = bcs->cs;
 	unsigned len = skb->len;
 	unsigned long flags;
 
-	if (bcs->proto2 == ISDN_PROTO_L2_HDLC)
-		skb = HDLC_Encode(skb, HW_HDR_LEN, 0);
+	if (bcs->proto2 == L2_HDLC)
+		skb = HDLC_Encode(skb);
 	else
-		skb = iraw_encode(skb, HW_HDR_LEN, 0);
+		skb = iraw_encode(skb);
 	if (!skb) {
-		dev_err(bcs->cs->dev,
+		dev_err(cs->dev,
 			"unable to allocate memory for encoding!\n");
 		return -ENOMEM;
 	}
 
 	skb_queue_tail(&bcs->squeue, skb);
-	spin_lock_irqsave(&bcs->cs->lock, flags);
-	if (bcs->cs->connected)
-		tasklet_schedule(&bcs->cs->write_tasklet);
-	spin_unlock_irqrestore(&bcs->cs->lock, flags);
+	spin_lock_irqsave(&cs->lock, flags);
+	if (cs->connected)
+		tasklet_schedule(&cs->write_tasklet);
+	spin_unlock_irqrestore(&cs->lock, flags);
 
 	return len;	/* ok so far */
 }
diff --git a/drivers/isdn/gigaset/bas-gigaset.c b/drivers/isdn/gigaset/bas-gigaset.c
index 5ed1d99eb9f3..95ebc5129895 100644
--- a/drivers/isdn/gigaset/bas-gigaset.c
+++ b/drivers/isdn/gigaset/bas-gigaset.c
@@ -57,7 +57,7 @@ MODULE_PARM_DESC(cidmode, "Call-ID mode");
 #define USB_SX353_PRODUCT_ID    0x0022
 
 /* table of devices that work with this driver */
-static const struct usb_device_id gigaset_table [] = {
+static const struct usb_device_id gigaset_table[] = {
 	{ USB_DEVICE(USB_GIGA_VENDOR_ID, USB_3070_PRODUCT_ID) },
 	{ USB_DEVICE(USB_GIGA_VENDOR_ID, USB_3075_PRODUCT_ID) },
 	{ USB_DEVICE(USB_GIGA_VENDOR_ID, USB_SX303_PRODUCT_ID) },
@@ -137,7 +137,7 @@ struct bas_cardstate {
 #define BS_RESETTING	0x200	/* waiting for HD_RESET_INTERRUPT_PIPE_ACK */
 
 
-static struct gigaset_driver *driver = NULL;
+static struct gigaset_driver *driver;
 
 /* usb specific object needed to register this driver with the usb subsystem */
 static struct usb_driver gigaset_usb_driver = {
@@ -601,11 +601,12 @@ static int atread_submit(struct cardstate *cs, int timeout)
 	ucs->dr_cmd_in.wLength = cpu_to_le16(ucs->rcvbuf_size);
 	usb_fill_control_urb(ucs->urb_cmd_in, ucs->udev,
 			     usb_rcvctrlpipe(ucs->udev, 0),
-			     (unsigned char*) & ucs->dr_cmd_in,
+			     (unsigned char *) &ucs->dr_cmd_in,
 			     ucs->rcvbuf, ucs->rcvbuf_size,
 			     read_ctrl_callback, cs->inbuf);
 
-	if ((ret = usb_submit_urb(ucs->urb_cmd_in, GFP_ATOMIC)) != 0) {
+	ret = usb_submit_urb(ucs->urb_cmd_in, GFP_ATOMIC);
+	if (ret != 0) {
 		update_basstate(ucs, 0, BS_ATRDPEND);
 		dev_err(cs->dev, "could not submit HD_READ_ATMESSAGE: %s\n",
 			get_usb_rcmsg(ret));
@@ -652,13 +653,11 @@ static void read_int_callback(struct urb *urb)
 		return;
 	case -ENODEV:			/* device removed */
 	case -ESHUTDOWN:		/* device shut down */
-		//FIXME use this as disconnect indicator?
 		gig_dbg(DEBUG_USBREQ, "%s: device disconnected", __func__);
 		return;
 	default:		/* severe trouble */
 		dev_warn(cs->dev, "interrupt read: %s\n",
 			 get_usb_statmsg(status));
-		//FIXME corrective action? resubmission always ok?
 		goto resubmit;
 	}
 
@@ -742,7 +741,8 @@ static void read_int_callback(struct urb *urb)
 			kfree(ucs->rcvbuf);
 			ucs->rcvbuf_size = 0;
 		}
-		if ((ucs->rcvbuf = kmalloc(l, GFP_ATOMIC)) == NULL) {
+		ucs->rcvbuf = kmalloc(l, GFP_ATOMIC);
+		if (ucs->rcvbuf == NULL) {
 			spin_unlock_irqrestore(&cs->lock, flags);
 			dev_err(cs->dev, "out of memory receiving AT data\n");
 			error_reset(cs);
@@ -750,12 +750,12 @@ static void read_int_callback(struct urb *urb)
 		}
 		ucs->rcvbuf_size = l;
 		ucs->retry_cmd_in = 0;
-		if ((rc = atread_submit(cs, BAS_TIMEOUT)) < 0) {
+		rc = atread_submit(cs, BAS_TIMEOUT);
+		if (rc < 0) {
 			kfree(ucs->rcvbuf);
 			ucs->rcvbuf = NULL;
 			ucs->rcvbuf_size = 0;
 			if (rc != -ENODEV) {
-				//FIXME corrective action?
 				spin_unlock_irqrestore(&cs->lock, flags);
 				error_reset(cs);
 				break;
@@ -911,7 +911,7 @@ static int starturbs(struct bc_state *bcs)
 	int rc;
 
 	/* initialize L2 reception */
-	if (bcs->proto2 == ISDN_PROTO_L2_HDLC)
+	if (bcs->proto2 == L2_HDLC)
 		bcs->inputstate |= INS_flag_hunt;
 
 	/* submit all isochronous input URBs */
@@ -940,7 +940,8 @@ static int starturbs(struct bc_state *bcs)
 		}
 
 		dump_urb(DEBUG_ISO, "Initial isoc read", urb);
-		if ((rc = usb_submit_urb(urb, GFP_ATOMIC)) != 0)
+		rc = usb_submit_urb(urb, GFP_ATOMIC);
+		if (rc != 0)
 			goto error;
 	}
 
@@ -1045,7 +1046,8 @@ static int submit_iso_write_urb(struct isow_urbctx_t *ucx)
 
 		/* compute frame length according to flow control */
 		ifd->length = BAS_NORMFRAME;
-		if ((corrbytes = atomic_read(&ubc->corrbytes)) != 0) {
+		corrbytes = atomic_read(&ubc->corrbytes);
+		if (corrbytes != 0) {
 			gig_dbg(DEBUG_ISO, "%s: corrbytes=%d",
 				__func__, corrbytes);
 			if (corrbytes > BAS_HIGHFRAME - BAS_NORMFRAME)
@@ -1064,7 +1066,7 @@ static int submit_iso_write_urb(struct isow_urbctx_t *ucx)
 					"%s: buffer busy at frame %d",
 					__func__, nframe);
 				/* tasklet will be restarted from
-				   gigaset_send_skb() */
+				   gigaset_isoc_send_skb() */
 			} else {
 				dev_err(ucx->bcs->cs->dev,
 					"%s: buffer error %d at frame %d\n",
@@ -1284,7 +1286,8 @@ static void read_iso_tasklet(unsigned long data)
 	for (;;) {
 		/* retrieve URB */
 		spin_lock_irqsave(&ubc->isoinlock, flags);
-		if (!(urb = ubc->isoindone)) {
+		urb = ubc->isoindone;
+		if (!urb) {
 			spin_unlock_irqrestore(&ubc->isoinlock, flags);
 			return;
 		}
@@ -1371,7 +1374,7 @@ static void read_iso_tasklet(unsigned long data)
 				 "isochronous read: %d data bytes missing\n",
 				 totleft);
 
-	error:
+error:
 		/* URB processed, resubmit */
 		for (frame = 0; frame < BAS_NUMFRAMES; frame++) {
 			urb->iso_frame_desc[frame].status = 0;
@@ -1568,7 +1571,7 @@ static int req_submit(struct bc_state *bcs, int req, int val, int timeout)
 	ucs->dr_ctrl.wLength = 0;
 	usb_fill_control_urb(ucs->urb_ctrl, ucs->udev,
 			     usb_sndctrlpipe(ucs->udev, 0),
-			     (unsigned char*) &ucs->dr_ctrl, NULL, 0,
+			     (unsigned char *) &ucs->dr_ctrl, NULL, 0,
 			     write_ctrl_callback, ucs);
 	ucs->retry_ctrl = 0;
 	ret = usb_submit_urb(ucs->urb_ctrl, GFP_ATOMIC);
@@ -1621,7 +1624,8 @@ static int gigaset_init_bchannel(struct bc_state *bcs)
 		return -EHOSTUNREACH;
 	}
 
-	if ((ret = starturbs(bcs)) < 0) {
+	ret = starturbs(bcs);
+	if (ret < 0) {
 		dev_err(cs->dev,
 			"could not start isochronous I/O for channel B%d: %s\n",
 			bcs->channel + 1,
@@ -1633,7 +1637,8 @@ static int gigaset_init_bchannel(struct bc_state *bcs)
 	}
 
 	req = bcs->channel ? HD_OPEN_B2CHANNEL : HD_OPEN_B1CHANNEL;
-	if ((ret = req_submit(bcs, req, 0, BAS_TIMEOUT)) < 0) {
+	ret = req_submit(bcs, req, 0, BAS_TIMEOUT);
+	if (ret < 0) {
 		dev_err(cs->dev, "could not open channel B%d\n",
 			bcs->channel + 1);
 		stopurbs(bcs->hw.bas);
@@ -1677,7 +1682,8 @@ static int gigaset_close_bchannel(struct bc_state *bcs)
 
 	/* channel running: tell device to close it */
 	req = bcs->channel ? HD_CLOSE_B2CHANNEL : HD_CLOSE_B1CHANNEL;
-	if ((ret = req_submit(bcs, req, 0, BAS_TIMEOUT)) < 0)
+	ret = req_submit(bcs, req, 0, BAS_TIMEOUT);
+	if (ret < 0)
 		dev_err(cs->dev, "closing channel B%d failed\n",
 			bcs->channel + 1);
 
@@ -1703,10 +1709,12 @@ static void complete_cb(struct cardstate *cs)
 	gig_dbg(DEBUG_TRANSCMD|DEBUG_LOCKCMD,
 		"write_command: sent %u bytes, %u left",
 		cs->curlen, cs->cmdbytes);
-	if ((cs->cmdbuf = cb->next) != NULL) {
+	if (cb->next != NULL) {
+		cs->cmdbuf = cb->next;
 		cs->cmdbuf->prev = NULL;
 		cs->curlen = cs->cmdbuf->len;
 	} else {
+		cs->cmdbuf = NULL;
 		cs->lastcmdbuf = NULL;
 		cs->curlen = 0;
 	}
@@ -1833,7 +1841,7 @@ static int atwrite_submit(struct cardstate *cs, unsigned char *buf, int len)
 	ucs->dr_cmd_out.wLength = cpu_to_le16(len);
 	usb_fill_control_urb(ucs->urb_cmd_out, ucs->udev,
 			     usb_sndctrlpipe(ucs->udev, 0),
-			     (unsigned char*) &ucs->dr_cmd_out, buf, len,
+			     (unsigned char *) &ucs->dr_cmd_out, buf, len,
 			     write_command_callback, cs);
 	rc = usb_submit_urb(ucs->urb_cmd_out, GFP_ATOMIC);
 	if (unlikely(rc)) {
@@ -1953,7 +1961,8 @@ static int gigaset_write_cmd(struct cardstate *cs,
 
 	if (len > IF_WRITEBUF)
 		len = IF_WRITEBUF;
-	if (!(cb = kmalloc(sizeof(struct cmdbuf_t) + len, GFP_ATOMIC))) {
+	cb = kmalloc(sizeof(struct cmdbuf_t) + len, GFP_ATOMIC);
+	if (!cb) {
 		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		rc = -ENOMEM;
 		goto notqueued;
@@ -2100,14 +2109,15 @@ static int gigaset_initbcshw(struct bc_state *bcs)
 	}
 	ubc->isooutdone = ubc->isooutfree = ubc->isooutovfl = NULL;
 	ubc->numsub = 0;
-	if (!(ubc->isooutbuf = kmalloc(sizeof(struct isowbuf_t), GFP_KERNEL))) {
+	ubc->isooutbuf = kmalloc(sizeof(struct isowbuf_t), GFP_KERNEL);
+	if (!ubc->isooutbuf) {
 		pr_err("out of memory\n");
 		kfree(ubc);
 		bcs->hw.bas = NULL;
 		return 0;
 	}
 	tasklet_init(&ubc->sent_tasklet,
-		     &write_iso_tasklet, (unsigned long) bcs);
+		     write_iso_tasklet, (unsigned long) bcs);
 
 	spin_lock_init(&ubc->isoinlock);
 	for (i = 0; i < BAS_INURBS; ++i)
@@ -2128,7 +2138,7 @@ static int gigaset_initbcshw(struct bc_state *bcs)
 	ubc->shared0s = 0;
 	ubc->stolen0s = 0;
 	tasklet_init(&ubc->rcvd_tasklet,
-		     &read_iso_tasklet, (unsigned long) bcs);
+		     read_iso_tasklet, (unsigned long) bcs);
 	return 1;
 }
 
@@ -2252,7 +2262,8 @@ static int gigaset_probe(struct usb_interface *interface,
 		gig_dbg(DEBUG_ANY,
 			"%s: wrong alternate setting %d - trying to switch",
 			__func__, hostif->desc.bAlternateSetting);
-		if (usb_set_interface(udev, hostif->desc.bInterfaceNumber, 3) < 0) {
+		if (usb_set_interface(udev, hostif->desc.bInterfaceNumber, 3)
+		    < 0) {
 			dev_warn(&udev->dev, "usb_set_interface failed, "
 				 "device %d interface %d altsetting %d\n",
 				 udev->devnum, hostif->desc.bInterfaceNumber,
@@ -2321,14 +2332,16 @@ static int gigaset_probe(struct usb_interface *interface,
 					(endpoint->bEndpointAddress) & 0x0f),
 			 ucs->int_in_buf, IP_MSGSIZE, read_int_callback, cs,
 			 endpoint->bInterval);
-	if ((rc = usb_submit_urb(ucs->urb_int_in, GFP_KERNEL)) != 0) {
+	rc = usb_submit_urb(ucs->urb_int_in, GFP_KERNEL);
+	if (rc != 0) {
 		dev_err(cs->dev, "could not submit interrupt URB: %s\n",
 			get_usb_rcmsg(rc));
 		goto error;
 	}
 
 	/* tell the device that the driver is ready */
-	if ((rc = req_submit(cs->bcs, HD_DEVICE_INIT_ACK, 0, 0)) != 0)
+	rc = req_submit(cs->bcs, HD_DEVICE_INIT_ACK, 0, 0);
+	if (rc != 0)
 		goto error;
 
 	/* tell common part that the device is ready */
@@ -2524,9 +2537,10 @@ static int __init bas_gigaset_init(void)
 	int result;
 
 	/* allocate memory for our driver state and intialize it */
-	if ((driver = gigaset_initdriver(GIGASET_MINOR, GIGASET_MINORS,
-				       GIGASET_MODULENAME, GIGASET_DEVNAME,
-				       &gigops, THIS_MODULE)) == NULL)
+	driver = gigaset_initdriver(GIGASET_MINOR, GIGASET_MINORS,
+				    GIGASET_MODULENAME, GIGASET_DEVNAME,
+				    &gigops, THIS_MODULE);
+	if (driver == NULL)
 		goto error;
 
 	/* register this driver with the USB subsystem */
diff --git a/drivers/isdn/gigaset/capi.c b/drivers/isdn/gigaset/capi.c
new file mode 100644
index 000000000000..3f5cd06af104
--- /dev/null
+++ b/drivers/isdn/gigaset/capi.c
@@ -0,0 +1,2292 @@
+/*
+ * Kernel CAPI interface for the Gigaset driver
+ *
+ * Copyright (c) 2009 by Tilman Schmidt <tilman@imap.cc>.
+ *
+ * =====================================================================
+ *	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.
+ * =====================================================================
+ */
+
+#include "gigaset.h"
+#include <linux/ctype.h>
+#include <linux/isdn/capilli.h>
+#include <linux/isdn/capicmd.h>
+#include <linux/isdn/capiutil.h>
+
+/* missing from kernelcapi.h */
+#define CapiNcpiNotSupportedByProtocol	0x0001
+#define CapiFlagsNotSupportedByProtocol	0x0002
+#define CapiAlertAlreadySent		0x0003
+#define CapiFacilitySpecificFunctionNotSupported	0x3011
+
+/* missing from capicmd.h */
+#define CAPI_CONNECT_IND_BASELEN	(CAPI_MSG_BASELEN+4+2+8*1)
+#define CAPI_CONNECT_ACTIVE_IND_BASELEN	(CAPI_MSG_BASELEN+4+3*1)
+#define CAPI_CONNECT_B3_IND_BASELEN	(CAPI_MSG_BASELEN+4+1)
+#define CAPI_CONNECT_B3_ACTIVE_IND_BASELEN	(CAPI_MSG_BASELEN+4+1)
+#define CAPI_DATA_B3_REQ_LEN64		(CAPI_MSG_BASELEN+4+4+2+2+2+8)
+#define CAPI_DATA_B3_CONF_LEN		(CAPI_MSG_BASELEN+4+2+2)
+#define CAPI_DISCONNECT_IND_LEN		(CAPI_MSG_BASELEN+4+2)
+#define CAPI_DISCONNECT_B3_IND_BASELEN	(CAPI_MSG_BASELEN+4+2+1)
+#define CAPI_FACILITY_CONF_BASELEN	(CAPI_MSG_BASELEN+4+2+2+1)
+/* most _CONF messages contain only Controller/PLCI/NCCI and Info parameters */
+#define CAPI_STDCONF_LEN		(CAPI_MSG_BASELEN+4+2)
+
+#define CAPI_FACILITY_HANDSET	0x0000
+#define CAPI_FACILITY_DTMF	0x0001
+#define CAPI_FACILITY_V42BIS	0x0002
+#define CAPI_FACILITY_SUPPSVC	0x0003
+#define CAPI_FACILITY_WAKEUP	0x0004
+#define CAPI_FACILITY_LI	0x0005
+
+#define CAPI_SUPPSVC_GETSUPPORTED	0x0000
+
+/* missing from capiutil.h */
+#define CAPIMSG_PLCI_PART(m)	CAPIMSG_U8(m, 9)
+#define CAPIMSG_NCCI_PART(m)	CAPIMSG_U16(m, 10)
+#define CAPIMSG_HANDLE_REQ(m)	CAPIMSG_U16(m, 18) /* DATA_B3_REQ/_IND only! */
+#define CAPIMSG_FLAGS(m)	CAPIMSG_U16(m, 20)
+#define CAPIMSG_SETCONTROLLER(m, contr)	capimsg_setu8(m, 8, contr)
+#define CAPIMSG_SETPLCI_PART(m, plci)	capimsg_setu8(m, 9, plci)
+#define CAPIMSG_SETNCCI_PART(m, ncci)	capimsg_setu16(m, 10, ncci)
+#define CAPIMSG_SETFLAGS(m, flags)	capimsg_setu16(m, 20, flags)
+
+/* parameters with differing location in DATA_B3_CONF/_RESP: */
+#define CAPIMSG_SETHANDLE_CONF(m, handle)	capimsg_setu16(m, 12, handle)
+#define	CAPIMSG_SETINFO_CONF(m, info)		capimsg_setu16(m, 14, info)
+
+/* Flags (DATA_B3_REQ/_IND) */
+#define CAPI_FLAGS_DELIVERY_CONFIRMATION	0x04
+#define CAPI_FLAGS_RESERVED			(~0x1f)
+
+/* buffer sizes */
+#define MAX_BC_OCTETS 11
+#define MAX_HLC_OCTETS 3
+#define MAX_NUMBER_DIGITS 20
+#define MAX_FMT_IE_LEN 20
+
+/* values for gigaset_capi_appl.connected */
+#define APCONN_NONE	0	/* inactive/listening */
+#define APCONN_SETUP	1	/* connecting */
+#define APCONN_ACTIVE	2	/* B channel up */
+
+/* registered application data structure */
+struct gigaset_capi_appl {
+	struct list_head ctrlist;
+	struct gigaset_capi_appl *bcnext;
+	u16 id;
+	u16 nextMessageNumber;
+	u32 listenInfoMask;
+	u32 listenCIPmask;
+	int connected;
+};
+
+/* CAPI specific controller data structure */
+struct gigaset_capi_ctr {
+	struct capi_ctr ctr;
+	struct list_head appls;
+	struct sk_buff_head sendqueue;
+	atomic_t sendqlen;
+	/* two _cmsg structures possibly used concurrently: */
+	_cmsg hcmsg;	/* for message composition triggered from hardware */
+	_cmsg acmsg;	/* for dissection of messages sent from application */
+	u8 bc_buf[MAX_BC_OCTETS+1];
+	u8 hlc_buf[MAX_HLC_OCTETS+1];
+	u8 cgpty_buf[MAX_NUMBER_DIGITS+3];
+	u8 cdpty_buf[MAX_NUMBER_DIGITS+2];
+};
+
+/* CIP Value table (from CAPI 2.0 standard, ch. 6.1) */
+static struct {
+	u8 *bc;
+	u8 *hlc;
+} cip2bchlc[] = {
+	[1] = { "8090A3", NULL },
+		/* Speech (A-law) */
+	[2] = { "8890", NULL },
+		/* Unrestricted digital information */
+	[3] = { "8990", NULL },
+		/* Restricted digital information */
+	[4] = { "9090A3", NULL },
+		/* 3,1 kHz audio (A-law) */
+	[5] = { "9190", NULL },
+		/* 7 kHz audio */
+	[6] = { "9890", NULL },
+		/* Video */
+	[7] = { "88C0C6E6", NULL },
+		/* Packet mode */
+	[8] = { "8890218F", NULL },
+		/* 56 kbit/s rate adaptation */
+	[9] = { "9190A5", NULL },
+		/* Unrestricted digital information with tones/announcements */
+	[16] = { "8090A3", "9181" },
+		/* Telephony */
+	[17] = { "9090A3", "9184" },
+		/* Group 2/3 facsimile */
+	[18] = { "8890", "91A1" },
+		/* Group 4 facsimile Class 1 */
+	[19] = { "8890", "91A4" },
+		/* Teletex service basic and mixed mode
+		   and Group 4 facsimile service Classes II and III */
+	[20] = { "8890", "91A8" },
+		/* Teletex service basic and processable mode */
+	[21] = { "8890", "91B1" },
+		/* Teletex service basic mode */
+	[22] = { "8890", "91B2" },
+		/* International interworking for Videotex */
+	[23] = { "8890", "91B5" },
+		/* Telex */
+	[24] = { "8890", "91B8" },
+		/* Message Handling Systems in accordance with X.400 */
+	[25] = { "8890", "91C1" },
+		/* OSI application in accordance with X.200 */
+	[26] = { "9190A5", "9181" },
+		/* 7 kHz telephony */
+	[27] = { "9190A5", "916001" },
+		/* Video telephony, first connection */
+	[28] = { "8890", "916002" },
+		/* Video telephony, second connection */
+};
+
+/*
+ * helper functions
+ * ================
+ */
+
+/*
+ * emit unsupported parameter warning
+ */
+static inline void ignore_cstruct_param(struct cardstate *cs, _cstruct param,
+				       char *msgname, char *paramname)
+{
+	if (param && *param)
+		dev_warn(cs->dev, "%s: ignoring unsupported parameter: %s\n",
+			 msgname, paramname);
+}
+
+/*
+ * check for legal hex digit
+ */
+static inline int ishexdigit(char c)
+{
+	if (c >= '0' && c <= '9')
+		return 1;
+	if (c >= 'A' && c <= 'F')
+		return 1;
+	if (c >= 'a' && c <= 'f')
+		return 1;
+	return 0;
+}
+
+/*
+ * convert hex to binary
+ */
+static inline u8 hex2bin(char c)
+{
+	int result = c & 0x0f;
+	if (c & 0x40)
+		result += 9;
+	return result;
+}
+
+/*
+ * convert an IE from Gigaset hex string to ETSI binary representation
+ * including length byte
+ * return value: result length, -1 on error
+ */
+static int encode_ie(char *in, u8 *out, int maxlen)
+{
+	int l = 0;
+	while (*in) {
+		if (!ishexdigit(in[0]) || !ishexdigit(in[1]) || l >= maxlen)
+			return -1;
+		out[++l] = (hex2bin(in[0]) << 4) + hex2bin(in[1]);
+		in += 2;
+	}
+	out[0] = l;
+	return l;
+}
+
+/*
+ * convert an IE from ETSI binary representation including length byte
+ * to Gigaset hex string
+ */
+static void decode_ie(u8 *in, char *out)
+{
+	int i = *in;
+	while (i-- > 0) {
+		/* ToDo: conversion to upper case necessary? */
+		*out++ = toupper(hex_asc_hi(*++in));
+		*out++ = toupper(hex_asc_lo(*in));
+	}
+}
+
+/*
+ * retrieve application data structure for an application ID
+ */
+static inline struct gigaset_capi_appl *
+get_appl(struct gigaset_capi_ctr *iif, u16 appl)
+{
+	struct gigaset_capi_appl *ap;
+
+	list_for_each_entry(ap, &iif->appls, ctrlist)
+		if (ap->id == appl)
+			return ap;
+	return NULL;
+}
+
+/*
+ * dump CAPI message to kernel messages for debugging
+ */
+static inline void dump_cmsg(enum debuglevel level, const char *tag, _cmsg *p)
+{
+#ifdef CONFIG_GIGASET_DEBUG
+	_cdebbuf *cdb;
+
+	if (!(gigaset_debuglevel & level))
+		return;
+
+	cdb = capi_cmsg2str(p);
+	if (cdb) {
+		gig_dbg(level, "%s: [%d] %s", tag, p->ApplId, cdb->buf);
+		cdebbuf_free(cdb);
+	} else {
+		gig_dbg(level, "%s: [%d] %s", tag, p->ApplId,
+			capi_cmd2str(p->Command, p->Subcommand));
+	}
+#endif
+}
+
+static inline void dump_rawmsg(enum debuglevel level, const char *tag,
+			       unsigned char *data)
+{
+#ifdef CONFIG_GIGASET_DEBUG
+	char *dbgline;
+	int i, l;
+
+	if (!(gigaset_debuglevel & level))
+		return;
+
+	l = CAPIMSG_LEN(data);
+	if (l < 12) {
+		gig_dbg(level, "%s: ??? LEN=%04d", tag, l);
+		return;
+	}
+	gig_dbg(level, "%s: 0x%02x:0x%02x: ID=%03d #0x%04x LEN=%04d NCCI=0x%x",
+		tag, CAPIMSG_COMMAND(data), CAPIMSG_SUBCOMMAND(data),
+		CAPIMSG_APPID(data), CAPIMSG_MSGID(data), l,
+		CAPIMSG_CONTROL(data));
+	l -= 12;
+	dbgline = kmalloc(3*l, GFP_ATOMIC);
+	if (!dbgline)
+		return;
+	for (i = 0; i < l; i++) {
+		dbgline[3*i] = hex_asc_hi(data[12+i]);
+		dbgline[3*i+1] = hex_asc_lo(data[12+i]);
+		dbgline[3*i+2] = ' ';
+	}
+	dbgline[3*l-1] = '\0';
+	gig_dbg(level, "  %s", dbgline);
+	kfree(dbgline);
+	if (CAPIMSG_COMMAND(data) == CAPI_DATA_B3 &&
+	    (CAPIMSG_SUBCOMMAND(data) == CAPI_REQ ||
+	     CAPIMSG_SUBCOMMAND(data) == CAPI_IND) &&
+	    CAPIMSG_DATALEN(data) > 0) {
+		l = CAPIMSG_DATALEN(data);
+		dbgline = kmalloc(3*l, GFP_ATOMIC);
+		if (!dbgline)
+			return;
+		data += CAPIMSG_LEN(data);
+		for (i = 0; i < l; i++) {
+			dbgline[3*i] = hex_asc_hi(data[i]);
+			dbgline[3*i+1] = hex_asc_lo(data[i]);
+			dbgline[3*i+2] = ' ';
+		}
+		dbgline[3*l-1] = '\0';
+		gig_dbg(level, "  %s", dbgline);
+		kfree(dbgline);
+	}
+#endif
+}
+
+/*
+ * format CAPI IE as string
+ */
+
+static const char *format_ie(const char *ie)
+{
+	static char result[3*MAX_FMT_IE_LEN];
+	int len, count;
+	char *pout = result;
+
+	if (!ie)
+		return "NULL";
+
+	count = len = ie[0];
+	if (count > MAX_FMT_IE_LEN)
+		count = MAX_FMT_IE_LEN-1;
+	while (count--) {
+		*pout++ = hex_asc_hi(*++ie);
+		*pout++ = hex_asc_lo(*ie);
+		*pout++ = ' ';
+	}
+	if (len > MAX_FMT_IE_LEN) {
+		*pout++ = '.';
+		*pout++ = '.';
+		*pout++ = '.';
+	}
+	*--pout = 0;
+	return result;
+}
+
+
+/*
+ * driver interface functions
+ * ==========================
+ */
+
+/**
+ * gigaset_skb_sent() - acknowledge transmission of outgoing skb
+ * @bcs:	B channel descriptor structure.
+ * @skb:	sent data.
+ *
+ * Called by hardware module {bas,ser,usb}_gigaset when the data in a
+ * skb has been successfully sent, for signalling completion to the LL.
+ */
+void gigaset_skb_sent(struct bc_state *bcs, struct sk_buff *dskb)
+{
+	struct cardstate *cs = bcs->cs;
+	struct gigaset_capi_ctr *iif = cs->iif;
+	struct gigaset_capi_appl *ap = bcs->ap;
+	unsigned char *req = skb_mac_header(dskb);
+	struct sk_buff *cskb;
+	u16 flags;
+
+	/* update statistics */
+	++bcs->trans_up;
+
+	if (!ap) {
+		dev_err(cs->dev, "%s: no application\n", __func__);
+		return;
+	}
+
+	/* don't send further B3 messages if disconnected */
+	if (ap->connected < APCONN_ACTIVE) {
+		gig_dbg(DEBUG_LLDATA, "disconnected, discarding ack");
+		return;
+	}
+
+	/* ToDo: honor unset "delivery confirmation" bit */
+	flags = CAPIMSG_FLAGS(req);
+
+	/* build DATA_B3_CONF message */
+	cskb = alloc_skb(CAPI_DATA_B3_CONF_LEN, GFP_ATOMIC);
+	if (!cskb) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		return;
+	}
+	/* frequent message, avoid _cmsg overhead */
+	CAPIMSG_SETLEN(cskb->data, CAPI_DATA_B3_CONF_LEN);
+	CAPIMSG_SETAPPID(cskb->data, ap->id);
+	CAPIMSG_SETCOMMAND(cskb->data, CAPI_DATA_B3);
+	CAPIMSG_SETSUBCOMMAND(cskb->data,  CAPI_CONF);
+	CAPIMSG_SETMSGID(cskb->data, CAPIMSG_MSGID(req));
+	CAPIMSG_SETCONTROLLER(cskb->data, iif->ctr.cnr);
+	CAPIMSG_SETPLCI_PART(cskb->data, bcs->channel + 1);
+	CAPIMSG_SETNCCI_PART(cskb->data, 1);
+	CAPIMSG_SETHANDLE_CONF(cskb->data, CAPIMSG_HANDLE_REQ(req));
+	if (flags & ~CAPI_FLAGS_DELIVERY_CONFIRMATION)
+		CAPIMSG_SETINFO_CONF(cskb->data,
+				     CapiFlagsNotSupportedByProtocol);
+	else
+		CAPIMSG_SETINFO_CONF(cskb->data, CAPI_NOERROR);
+
+	/* emit message */
+	dump_rawmsg(DEBUG_LLDATA, "DATA_B3_CONF", cskb->data);
+	capi_ctr_handle_message(&iif->ctr, ap->id, cskb);
+}
+EXPORT_SYMBOL_GPL(gigaset_skb_sent);
+
+/**
+ * gigaset_skb_rcvd() - pass received skb to LL
+ * @bcs:	B channel descriptor structure.
+ * @skb:	received data.
+ *
+ * Called by hardware module {bas,ser,usb}_gigaset when user data has
+ * been successfully received, for passing to the LL.
+ * Warning: skb must not be accessed anymore!
+ */
+void gigaset_skb_rcvd(struct bc_state *bcs, struct sk_buff *skb)
+{
+	struct cardstate *cs = bcs->cs;
+	struct gigaset_capi_ctr *iif = cs->iif;
+	struct gigaset_capi_appl *ap = bcs->ap;
+	int len = skb->len;
+
+	/* update statistics */
+	bcs->trans_down++;
+
+	if (!ap) {
+		dev_err(cs->dev, "%s: no application\n", __func__);
+		return;
+	}
+
+	/* don't send further B3 messages if disconnected */
+	if (ap->connected < APCONN_ACTIVE) {
+		gig_dbg(DEBUG_LLDATA, "disconnected, discarding data");
+		dev_kfree_skb_any(skb);
+		return;
+	}
+
+	/*
+	 * prepend DATA_B3_IND message to payload
+	 * Parameters: NCCI = 1, all others 0/unused
+	 * frequent message, avoid _cmsg overhead
+	 */
+	skb_push(skb, CAPI_DATA_B3_REQ_LEN);
+	CAPIMSG_SETLEN(skb->data, CAPI_DATA_B3_REQ_LEN);
+	CAPIMSG_SETAPPID(skb->data, ap->id);
+	CAPIMSG_SETCOMMAND(skb->data, CAPI_DATA_B3);
+	CAPIMSG_SETSUBCOMMAND(skb->data,  CAPI_IND);
+	CAPIMSG_SETMSGID(skb->data, ap->nextMessageNumber++);
+	CAPIMSG_SETCONTROLLER(skb->data, iif->ctr.cnr);
+	CAPIMSG_SETPLCI_PART(skb->data, bcs->channel + 1);
+	CAPIMSG_SETNCCI_PART(skb->data, 1);
+	/* Data parameter not used */
+	CAPIMSG_SETDATALEN(skb->data, len);
+	/* Data handle parameter not used */
+	CAPIMSG_SETFLAGS(skb->data, 0);
+	/* Data64 parameter not present */
+
+	/* emit message */
+	dump_rawmsg(DEBUG_LLDATA, "DATA_B3_IND", skb->data);
+	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+}
+EXPORT_SYMBOL_GPL(gigaset_skb_rcvd);
+
+/**
+ * gigaset_isdn_rcv_err() - signal receive error
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by hardware module {bas,ser,usb}_gigaset when a receive error
+ * has occurred, for signalling to the LL.
+ */
+void gigaset_isdn_rcv_err(struct bc_state *bcs)
+{
+	/* if currently ignoring packets, just count down */
+	if (bcs->ignore) {
+		bcs->ignore--;
+		return;
+	}
+
+	/* update statistics */
+	bcs->corrupted++;
+
+	/* ToDo: signal error -> LL */
+}
+EXPORT_SYMBOL_GPL(gigaset_isdn_rcv_err);
+
+/**
+ * gigaset_isdn_icall() - signal incoming call
+ * @at_state:	connection state structure.
+ *
+ * Called by main module at tasklet level to notify the LL that an incoming
+ * call has been received. @at_state contains the parameters of the call.
+ *
+ * Return value: call disposition (ICALL_*)
+ */
+int gigaset_isdn_icall(struct at_state_t *at_state)
+{
+	struct cardstate *cs = at_state->cs;
+	struct bc_state *bcs = at_state->bcs;
+	struct gigaset_capi_ctr *iif = cs->iif;
+	struct gigaset_capi_appl *ap;
+	u32 actCIPmask;
+	struct sk_buff *skb;
+	unsigned int msgsize;
+	int i;
+
+	/*
+	 * ToDo: signal calls without a free B channel, too
+	 * (requires a u8 handle for the at_state structure that can
+	 * be stored in the PLCI and used in the CONNECT_RESP message
+	 * handler to retrieve it)
+	 */
+	if (!bcs)
+		return ICALL_IGNORE;
+
+	/* prepare CONNECT_IND message, using B channel number as PLCI */
+	capi_cmsg_header(&iif->hcmsg, 0, CAPI_CONNECT, CAPI_IND, 0,
+			 iif->ctr.cnr | ((bcs->channel + 1) << 8));
+
+	/* minimum size, all structs empty */
+	msgsize = CAPI_CONNECT_IND_BASELEN;
+
+	/* Bearer Capability (mandatory) */
+	if (at_state->str_var[STR_ZBC]) {
+		/* pass on BC from Gigaset */
+		if (encode_ie(at_state->str_var[STR_ZBC], iif->bc_buf,
+			      MAX_BC_OCTETS) < 0) {
+			dev_warn(cs->dev, "RING ignored - bad BC %s\n",
+				 at_state->str_var[STR_ZBC]);
+			return ICALL_IGNORE;
+		}
+
+		/* look up corresponding CIP value */
+		iif->hcmsg.CIPValue = 0;	/* default if nothing found */
+		for (i = 0; i < ARRAY_SIZE(cip2bchlc); i++)
+			if (cip2bchlc[i].bc != NULL &&
+			    cip2bchlc[i].hlc == NULL &&
+			    !strcmp(cip2bchlc[i].bc,
+				    at_state->str_var[STR_ZBC])) {
+				iif->hcmsg.CIPValue = i;
+				break;
+			}
+	} else {
+		/* no BC (internal call): assume CIP 1 (speech, A-law) */
+		iif->hcmsg.CIPValue = 1;
+		encode_ie(cip2bchlc[1].bc, iif->bc_buf, MAX_BC_OCTETS);
+	}
+	iif->hcmsg.BC = iif->bc_buf;
+	msgsize += iif->hcmsg.BC[0];
+
+	/* High Layer Compatibility (optional) */
+	if (at_state->str_var[STR_ZHLC]) {
+		/* pass on HLC from Gigaset */
+		if (encode_ie(at_state->str_var[STR_ZHLC], iif->hlc_buf,
+			      MAX_HLC_OCTETS) < 0) {
+			dev_warn(cs->dev, "RING ignored - bad HLC %s\n",
+				 at_state->str_var[STR_ZHLC]);
+			return ICALL_IGNORE;
+		}
+		iif->hcmsg.HLC = iif->hlc_buf;
+		msgsize += iif->hcmsg.HLC[0];
+
+		/* look up corresponding CIP value */
+		/* keep BC based CIP value if none found */
+		if (at_state->str_var[STR_ZBC])
+			for (i = 0; i < ARRAY_SIZE(cip2bchlc); i++)
+				if (cip2bchlc[i].hlc != NULL &&
+				    !strcmp(cip2bchlc[i].hlc,
+					    at_state->str_var[STR_ZHLC]) &&
+				    !strcmp(cip2bchlc[i].bc,
+					    at_state->str_var[STR_ZBC])) {
+					iif->hcmsg.CIPValue = i;
+					break;
+				}
+	}
+
+	/* Called Party Number (optional) */
+	if (at_state->str_var[STR_ZCPN]) {
+		i = strlen(at_state->str_var[STR_ZCPN]);
+		if (i > MAX_NUMBER_DIGITS) {
+			dev_warn(cs->dev, "RING ignored - bad number %s\n",
+				 at_state->str_var[STR_ZBC]);
+			return ICALL_IGNORE;
+		}
+		iif->cdpty_buf[0] = i + 1;
+		iif->cdpty_buf[1] = 0x80; /* type / numbering plan unknown */
+		memcpy(iif->cdpty_buf+2, at_state->str_var[STR_ZCPN], i);
+		iif->hcmsg.CalledPartyNumber = iif->cdpty_buf;
+		msgsize += iif->hcmsg.CalledPartyNumber[0];
+	}
+
+	/* Calling Party Number (optional) */
+	if (at_state->str_var[STR_NMBR]) {
+		i = strlen(at_state->str_var[STR_NMBR]);
+		if (i > MAX_NUMBER_DIGITS) {
+			dev_warn(cs->dev, "RING ignored - bad number %s\n",
+				 at_state->str_var[STR_ZBC]);
+			return ICALL_IGNORE;
+		}
+		iif->cgpty_buf[0] = i + 2;
+		iif->cgpty_buf[1] = 0x00; /* type / numbering plan unknown */
+		iif->cgpty_buf[2] = 0x80; /* pres. allowed, not screened */
+		memcpy(iif->cgpty_buf+3, at_state->str_var[STR_NMBR], i);
+		iif->hcmsg.CallingPartyNumber = iif->cgpty_buf;
+		msgsize += iif->hcmsg.CallingPartyNumber[0];
+	}
+
+	/* remaining parameters (not supported, always left NULL):
+	 * - CalledPartySubaddress
+	 * - CallingPartySubaddress
+	 * - AdditionalInfo
+	 *   - BChannelinformation
+	 *   - Keypadfacility
+	 *   - Useruserdata
+	 *   - Facilitydataarray
+	 */
+
+	gig_dbg(DEBUG_CMD, "icall: PLCI %x CIP %d BC %s",
+		iif->hcmsg.adr.adrPLCI, iif->hcmsg.CIPValue,
+		format_ie(iif->hcmsg.BC));
+	gig_dbg(DEBUG_CMD, "icall: HLC %s",
+		format_ie(iif->hcmsg.HLC));
+	gig_dbg(DEBUG_CMD, "icall: CgPty %s",
+		format_ie(iif->hcmsg.CallingPartyNumber));
+	gig_dbg(DEBUG_CMD, "icall: CdPty %s",
+		format_ie(iif->hcmsg.CalledPartyNumber));
+
+	/* scan application list for matching listeners */
+	bcs->ap = NULL;
+	actCIPmask = 1 | (1 << iif->hcmsg.CIPValue);
+	list_for_each_entry(ap, &iif->appls, ctrlist)
+		if (actCIPmask & ap->listenCIPmask) {
+			/* build CONNECT_IND message for this application */
+			iif->hcmsg.ApplId = ap->id;
+			iif->hcmsg.Messagenumber = ap->nextMessageNumber++;
+
+			skb = alloc_skb(msgsize, GFP_ATOMIC);
+			if (!skb) {
+				dev_err(cs->dev, "%s: out of memory\n",
+					__func__);
+				break;
+			}
+			capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize));
+			dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
+
+			/* add to listeners on this B channel, update state */
+			ap->bcnext = bcs->ap;
+			bcs->ap = ap;
+			bcs->chstate |= CHS_NOTIFY_LL;
+			ap->connected = APCONN_SETUP;
+
+			/* emit message */
+			capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+		}
+
+	/*
+	 * Return "accept" if any listeners.
+	 * Gigaset will send ALERTING.
+	 * There doesn't seem to be a way to avoid this.
+	 */
+	return bcs->ap ? ICALL_ACCEPT : ICALL_IGNORE;
+}
+
+/*
+ * send a DISCONNECT_IND message to an application
+ * does not sleep, clobbers the controller's hcmsg structure
+ */
+static void send_disconnect_ind(struct bc_state *bcs,
+				struct gigaset_capi_appl *ap, u16 reason)
+{
+	struct cardstate *cs = bcs->cs;
+	struct gigaset_capi_ctr *iif = cs->iif;
+	struct sk_buff *skb;
+
+	if (ap->connected == APCONN_NONE)
+		return;
+
+	capi_cmsg_header(&iif->hcmsg, ap->id, CAPI_DISCONNECT, CAPI_IND,
+			 ap->nextMessageNumber++,
+			 iif->ctr.cnr | ((bcs->channel + 1) << 8));
+	iif->hcmsg.Reason = reason;
+	skb = alloc_skb(CAPI_DISCONNECT_IND_LEN, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		return;
+	}
+	capi_cmsg2message(&iif->hcmsg, __skb_put(skb, CAPI_DISCONNECT_IND_LEN));
+	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
+	ap->connected = APCONN_NONE;
+	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+}
+
+/*
+ * send a DISCONNECT_B3_IND message to an application
+ * Parameters: NCCI = 1, NCPI empty, Reason_B3 = 0
+ * does not sleep, clobbers the controller's hcmsg structure
+ */
+static void send_disconnect_b3_ind(struct bc_state *bcs,
+				   struct gigaset_capi_appl *ap)
+{
+	struct cardstate *cs = bcs->cs;
+	struct gigaset_capi_ctr *iif = cs->iif;
+	struct sk_buff *skb;
+
+	/* nothing to do if no logical connection active */
+	if (ap->connected < APCONN_ACTIVE)
+		return;
+	ap->connected = APCONN_SETUP;
+
+	capi_cmsg_header(&iif->hcmsg, ap->id, CAPI_DISCONNECT_B3, CAPI_IND,
+			 ap->nextMessageNumber++,
+			 iif->ctr.cnr | ((bcs->channel + 1) << 8) | (1 << 16));
+	skb = alloc_skb(CAPI_DISCONNECT_B3_IND_BASELEN, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		return;
+	}
+	capi_cmsg2message(&iif->hcmsg,
+			  __skb_put(skb, CAPI_DISCONNECT_B3_IND_BASELEN));
+	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
+	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+}
+
+/**
+ * gigaset_isdn_connD() - signal D channel connect
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module at tasklet level to notify the LL that the D channel
+ * connection has been established.
+ */
+void gigaset_isdn_connD(struct bc_state *bcs)
+{
+	struct cardstate *cs = bcs->cs;
+	struct gigaset_capi_ctr *iif = cs->iif;
+	struct gigaset_capi_appl *ap = bcs->ap;
+	struct sk_buff *skb;
+	unsigned int msgsize;
+
+	if (!ap) {
+		dev_err(cs->dev, "%s: no application\n", __func__);
+		return;
+	}
+	while (ap->bcnext) {
+		/* this should never happen */
+		dev_warn(cs->dev, "%s: dropping extra application %u\n",
+			 __func__, ap->bcnext->id);
+		send_disconnect_ind(bcs, ap->bcnext,
+				    CapiCallGivenToOtherApplication);
+		ap->bcnext = ap->bcnext->bcnext;
+	}
+	if (ap->connected == APCONN_NONE) {
+		dev_warn(cs->dev, "%s: application %u not connected\n",
+			 __func__, ap->id);
+		return;
+	}
+
+	/* prepare CONNECT_ACTIVE_IND message
+	 * Note: LLC not supported by device
+	 */
+	capi_cmsg_header(&iif->hcmsg, ap->id, CAPI_CONNECT_ACTIVE, CAPI_IND,
+			 ap->nextMessageNumber++,
+			 iif->ctr.cnr | ((bcs->channel + 1) << 8));
+
+	/* minimum size, all structs empty */
+	msgsize = CAPI_CONNECT_ACTIVE_IND_BASELEN;
+
+	/* ToDo: set parameter: Connected number
+	 * (requires ev-layer state machine extension to collect
+	 * ZCON device reply)
+	 */
+
+	/* build and emit CONNECT_ACTIVE_IND message */
+	skb = alloc_skb(msgsize, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		return;
+	}
+	capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize));
+	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
+	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+}
+
+/**
+ * gigaset_isdn_hupD() - signal D channel hangup
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module at tasklet level to notify the LL that the D channel
+ * connection has been shut down.
+ */
+void gigaset_isdn_hupD(struct bc_state *bcs)
+{
+	struct gigaset_capi_appl *ap;
+
+	/*
+	 * ToDo: pass on reason code reported by device
+	 * (requires ev-layer state machine extension to collect
+	 * ZCAU device reply)
+	 */
+	for (ap = bcs->ap; ap != NULL; ap = ap->bcnext) {
+		send_disconnect_b3_ind(bcs, ap);
+		send_disconnect_ind(bcs, ap, 0);
+	}
+	bcs->ap = NULL;
+}
+
+/**
+ * gigaset_isdn_connB() - signal B channel connect
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module at tasklet level to notify the LL that the B channel
+ * connection has been established.
+ */
+void gigaset_isdn_connB(struct bc_state *bcs)
+{
+	struct cardstate *cs = bcs->cs;
+	struct gigaset_capi_ctr *iif = cs->iif;
+	struct gigaset_capi_appl *ap = bcs->ap;
+	struct sk_buff *skb;
+	unsigned int msgsize;
+	u8 command;
+
+	if (!ap) {
+		dev_err(cs->dev, "%s: no application\n", __func__);
+		return;
+	}
+	while (ap->bcnext) {
+		/* this should never happen */
+		dev_warn(cs->dev, "%s: dropping extra application %u\n",
+			 __func__, ap->bcnext->id);
+		send_disconnect_ind(bcs, ap->bcnext,
+				    CapiCallGivenToOtherApplication);
+		ap->bcnext = ap->bcnext->bcnext;
+	}
+	if (!ap->connected) {
+		dev_warn(cs->dev, "%s: application %u not connected\n",
+			 __func__, ap->id);
+		return;
+	}
+
+	/*
+	 * emit CONNECT_B3_ACTIVE_IND if we already got CONNECT_B3_REQ;
+	 * otherwise we have to emit CONNECT_B3_IND first, and follow up with
+	 * CONNECT_B3_ACTIVE_IND in reply to CONNECT_B3_RESP
+	 * Parameters in both cases always: NCCI = 1, NCPI empty
+	 */
+	if (ap->connected >= APCONN_ACTIVE) {
+		command = CAPI_CONNECT_B3_ACTIVE;
+		msgsize = CAPI_CONNECT_B3_ACTIVE_IND_BASELEN;
+	} else {
+		command = CAPI_CONNECT_B3;
+		msgsize = CAPI_CONNECT_B3_IND_BASELEN;
+	}
+	capi_cmsg_header(&iif->hcmsg, ap->id, command, CAPI_IND,
+			 ap->nextMessageNumber++,
+			 iif->ctr.cnr | ((bcs->channel + 1) << 8) | (1 << 16));
+	skb = alloc_skb(msgsize, GFP_ATOMIC);
+	if (!skb) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		return;
+	}
+	capi_cmsg2message(&iif->hcmsg, __skb_put(skb, msgsize));
+	dump_cmsg(DEBUG_CMD, __func__, &iif->hcmsg);
+	ap->connected = APCONN_ACTIVE;
+	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+}
+
+/**
+ * gigaset_isdn_hupB() - signal B channel hangup
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module to notify the LL that the B channel connection has
+ * been shut down.
+ */
+void gigaset_isdn_hupB(struct bc_state *bcs)
+{
+	struct cardstate *cs = bcs->cs;
+	struct gigaset_capi_appl *ap = bcs->ap;
+
+	/* ToDo: assure order of DISCONNECT_B3_IND and DISCONNECT_IND ? */
+
+	if (!ap) {
+		dev_err(cs->dev, "%s: no application\n", __func__);
+		return;
+	}
+
+	send_disconnect_b3_ind(bcs, ap);
+}
+
+/**
+ * gigaset_isdn_start() - signal device availability
+ * @cs:		device descriptor structure.
+ *
+ * Called by main module to notify the LL that the device is available for
+ * use.
+ */
+void gigaset_isdn_start(struct cardstate *cs)
+{
+	struct gigaset_capi_ctr *iif = cs->iif;
+
+	/* fill profile data: manufacturer name */
+	strcpy(iif->ctr.manu, "Siemens");
+	/* CAPI and device version */
+	iif->ctr.version.majorversion = 2;		/* CAPI 2.0 */
+	iif->ctr.version.minorversion = 0;
+	/* ToDo: check/assert cs->gotfwver? */
+	iif->ctr.version.majormanuversion = cs->fwver[0];
+	iif->ctr.version.minormanuversion = cs->fwver[1];
+	/* number of B channels supported */
+	iif->ctr.profile.nbchannel = cs->channels;
+	/* global options: internal controller, supplementary services */
+	iif->ctr.profile.goptions = 0x11;
+	/* B1 protocols: 64 kbit/s HDLC or transparent */
+	iif->ctr.profile.support1 =  0x03;
+	/* B2 protocols: transparent only */
+	/* ToDo: X.75 SLP ? */
+	iif->ctr.profile.support2 =  0x02;
+	/* B3 protocols: transparent only */
+	iif->ctr.profile.support3 =  0x01;
+	/* no serial number */
+	strcpy(iif->ctr.serial, "0");
+	capi_ctr_ready(&iif->ctr);
+}
+
+/**
+ * gigaset_isdn_stop() - signal device unavailability
+ * @cs:		device descriptor structure.
+ *
+ * Called by main module to notify the LL that the device is no longer
+ * available for use.
+ */
+void gigaset_isdn_stop(struct cardstate *cs)
+{
+	struct gigaset_capi_ctr *iif = cs->iif;
+	capi_ctr_down(&iif->ctr);
+}
+
+/*
+ * kernel CAPI callback methods
+ * ============================
+ */
+
+/*
+ * load firmware
+ */
+static int gigaset_load_firmware(struct capi_ctr *ctr, capiloaddata *data)
+{
+	struct cardstate *cs = ctr->driverdata;
+
+	/* AVM specific operation, not needed for Gigaset -- ignore */
+	dev_notice(cs->dev, "load_firmware ignored\n");
+
+	return 0;
+}
+
+/*
+ * reset (deactivate) controller
+ */
+static void gigaset_reset_ctr(struct capi_ctr *ctr)
+{
+	struct cardstate *cs = ctr->driverdata;
+
+	/* AVM specific operation, not needed for Gigaset -- ignore */
+	dev_notice(cs->dev, "reset_ctr ignored\n");
+}
+
+/*
+ * register CAPI application
+ */
+static void gigaset_register_appl(struct capi_ctr *ctr, u16 appl,
+			   capi_register_params *rp)
+{
+	struct gigaset_capi_ctr *iif
+		= container_of(ctr, struct gigaset_capi_ctr, ctr);
+	struct cardstate *cs = ctr->driverdata;
+	struct gigaset_capi_appl *ap;
+
+	list_for_each_entry(ap, &iif->appls, ctrlist)
+		if (ap->id == appl) {
+			dev_notice(cs->dev,
+				   "application %u already registered\n", appl);
+			return;
+		}
+
+	ap = kzalloc(sizeof(*ap), GFP_KERNEL);
+	if (!ap) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		return;
+	}
+	ap->id = appl;
+
+	list_add(&ap->ctrlist, &iif->appls);
+}
+
+/*
+ * release CAPI application
+ */
+static void gigaset_release_appl(struct capi_ctr *ctr, u16 appl)
+{
+	struct gigaset_capi_ctr *iif
+		= container_of(ctr, struct gigaset_capi_ctr, ctr);
+	struct cardstate *cs = iif->ctr.driverdata;
+	struct gigaset_capi_appl *ap, *tmp;
+
+	list_for_each_entry_safe(ap, tmp, &iif->appls, ctrlist)
+		if (ap->id == appl) {
+			if (ap->connected != APCONN_NONE) {
+				dev_err(cs->dev,
+					"%s: application %u still connected\n",
+					__func__, ap->id);
+				/* ToDo: clear active connection */
+			}
+			list_del(&ap->ctrlist);
+			kfree(ap);
+		}
+
+}
+
+/*
+ * =====================================================================
+ * outgoing CAPI message handler
+ * =====================================================================
+ */
+
+/*
+ * helper function: emit reply message with given Info value
+ */
+static void send_conf(struct gigaset_capi_ctr *iif,
+		      struct gigaset_capi_appl *ap,
+		      struct sk_buff *skb,
+		      u16 info)
+{
+	/*
+	 * _CONF replies always only have NCCI and Info parameters
+	 * so they'll fit into the _REQ message skb
+	 */
+	capi_cmsg_answer(&iif->acmsg);
+	iif->acmsg.Info = info;
+	capi_cmsg2message(&iif->acmsg, skb->data);
+	__skb_trim(skb, CAPI_STDCONF_LEN);
+	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
+	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+}
+
+/*
+ * process FACILITY_REQ message
+ */
+static void do_facility_req(struct gigaset_capi_ctr *iif,
+			    struct gigaset_capi_appl *ap,
+			    struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	_cmsg *cmsg = &iif->acmsg;
+	struct sk_buff *cskb;
+	u8 *pparam;
+	unsigned int msgsize = CAPI_FACILITY_CONF_BASELEN;
+	u16 function, info;
+	static u8 confparam[10];	/* max. 9 octets + length byte */
+
+	/* decode message */
+	capi_message2cmsg(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+
+	/*
+	 * Facility Request Parameter is not decoded by capi_message2cmsg()
+	 * encoding depends on Facility Selector
+	 */
+	switch (cmsg->FacilitySelector) {
+	case CAPI_FACILITY_DTMF:	/* ToDo */
+		info = CapiFacilityNotSupported;
+		confparam[0] = 2;	/* length */
+		/* DTMF information: Unknown DTMF request */
+		capimsg_setu16(confparam, 1, 2);
+		break;
+
+	case CAPI_FACILITY_V42BIS:	/* not supported */
+		info = CapiFacilityNotSupported;
+		confparam[0] = 2;	/* length */
+		/* V.42 bis information: not available */
+		capimsg_setu16(confparam, 1, 1);
+		break;
+
+	case CAPI_FACILITY_SUPPSVC:
+		/* decode Function parameter */
+		pparam = cmsg->FacilityRequestParameter;
+		if (pparam == NULL || *pparam < 2) {
+			dev_notice(cs->dev, "%s: %s missing\n", "FACILITY_REQ",
+				   "Facility Request Parameter");
+			send_conf(iif, ap, skb, CapiIllMessageParmCoding);
+			return;
+		}
+		function = CAPIMSG_U16(pparam, 1);
+		switch (function) {
+		case CAPI_SUPPSVC_GETSUPPORTED:
+			info = CapiSuccess;
+			/* Supplementary Service specific parameter */
+			confparam[3] = 6;	/* length */
+			/* Supplementary services info: Success */
+			capimsg_setu16(confparam, 4, CapiSuccess);
+			/* Supported Services: none */
+			capimsg_setu32(confparam, 6, 0);
+			break;
+		/* ToDo: add supported services */
+		default:
+			info = CapiFacilitySpecificFunctionNotSupported;
+			/* Supplementary Service specific parameter */
+			confparam[3] = 2;	/* length */
+			/* Supplementary services info: not supported */
+			capimsg_setu16(confparam, 4,
+				       CapiSupplementaryServiceNotSupported);
+		}
+
+		/* Facility confirmation parameter */
+		confparam[0] = confparam[3] + 3;	/* total length */
+		/* Function: copy from _REQ message */
+		capimsg_setu16(confparam, 1, function);
+		/* Supplementary Service specific parameter already set above */
+		break;
+
+	case CAPI_FACILITY_WAKEUP:	/* ToDo */
+		info = CapiFacilityNotSupported;
+		confparam[0] = 2;	/* length */
+		/* Number of accepted awake request parameters: 0 */
+		capimsg_setu16(confparam, 1, 0);
+		break;
+
+	default:
+		info = CapiFacilityNotSupported;
+		confparam[0] = 0;	/* empty struct */
+	}
+
+	/* send FACILITY_CONF with given Info and confirmation parameter */
+	capi_cmsg_answer(cmsg);
+	cmsg->Info = info;
+	cmsg->FacilityConfirmationParameter = confparam;
+	msgsize += confparam[0];	/* length */
+	cskb = alloc_skb(msgsize, GFP_ATOMIC);
+	if (!cskb) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		return;
+	}
+	capi_cmsg2message(cmsg, __skb_put(cskb, msgsize));
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+	capi_ctr_handle_message(&iif->ctr, ap->id, cskb);
+}
+
+
+/*
+ * process LISTEN_REQ message
+ * just store the masks in the application data structure
+ */
+static void do_listen_req(struct gigaset_capi_ctr *iif,
+			  struct gigaset_capi_appl *ap,
+			  struct sk_buff *skb)
+{
+	/* decode message */
+	capi_message2cmsg(&iif->acmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
+
+	/* store listening parameters */
+	ap->listenInfoMask = iif->acmsg.InfoMask;
+	ap->listenCIPmask = iif->acmsg.CIPmask;
+	send_conf(iif, ap, skb, CapiSuccess);
+}
+
+/*
+ * process ALERT_REQ message
+ * nothing to do, Gigaset always alerts anyway
+ */
+static void do_alert_req(struct gigaset_capi_ctr *iif,
+			 struct gigaset_capi_appl *ap,
+			 struct sk_buff *skb)
+{
+	/* decode message */
+	capi_message2cmsg(&iif->acmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
+	send_conf(iif, ap, skb, CapiAlertAlreadySent);
+}
+
+/*
+ * process CONNECT_REQ message
+ * allocate a B channel, prepare dial commands, queue a DIAL event,
+ * emit CONNECT_CONF reply
+ */
+static void do_connect_req(struct gigaset_capi_ctr *iif,
+			   struct gigaset_capi_appl *ap,
+			   struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	_cmsg *cmsg = &iif->acmsg;
+	struct bc_state *bcs;
+	char **commands;
+	char *s;
+	u8 *pp;
+	int i, l;
+	u16 info;
+
+	/* decode message */
+	capi_message2cmsg(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+
+	/* get free B channel & construct PLCI */
+	bcs = gigaset_get_free_channel(cs);
+	if (!bcs) {
+		dev_notice(cs->dev, "%s: no B channel available\n",
+			   "CONNECT_REQ");
+		send_conf(iif, ap, skb, CapiNoPlciAvailable);
+		return;
+	}
+	ap->bcnext = NULL;
+	bcs->ap = ap;
+	cmsg->adr.adrPLCI |= (bcs->channel + 1) << 8;
+
+	/* build command table */
+	commands = kzalloc(AT_NUM*(sizeof *commands), GFP_KERNEL);
+	if (!commands)
+		goto oom;
+
+	/* encode parameter: Called party number */
+	pp = cmsg->CalledPartyNumber;
+	if (pp == NULL || *pp == 0) {
+		dev_notice(cs->dev, "%s: %s missing\n",
+			   "CONNECT_REQ", "Called party number");
+		info = CapiIllMessageParmCoding;
+		goto error;
+	}
+	l = *pp++;
+	/* check type of number/numbering plan byte */
+	switch (*pp) {
+	case 0x80:	/* unknown type / unknown numbering plan */
+	case 0x81:	/* unknown type / ISDN/Telephony numbering plan */
+		break;
+	default:	/* others: warn about potential misinterpretation */
+		dev_notice(cs->dev, "%s: %s type/plan 0x%02x unsupported\n",
+			   "CONNECT_REQ", "Called party number", *pp);
+	}
+	pp++;
+	l--;
+	/* translate "**" internal call prefix to CTP value */
+	if (l >= 2 && pp[0] == '*' && pp[1] == '*') {
+		s = "^SCTP=0\r";
+		pp += 2;
+		l -= 2;
+	} else {
+		s = "^SCTP=1\r";
+	}
+	commands[AT_TYPE] = kstrdup(s, GFP_KERNEL);
+	if (!commands[AT_TYPE])
+		goto oom;
+	commands[AT_DIAL] = kmalloc(l+3, GFP_KERNEL);
+	if (!commands[AT_DIAL])
+		goto oom;
+	snprintf(commands[AT_DIAL], l+3, "D%.*s\r", l, pp);
+
+	/* encode parameter: Calling party number */
+	pp = cmsg->CallingPartyNumber;
+	if (pp != NULL && *pp > 0) {
+		l = *pp++;
+
+		/* check type of number/numbering plan byte */
+		/* ToDo: allow for/handle Ext=1? */
+		switch (*pp) {
+		case 0x00:	/* unknown type / unknown numbering plan */
+		case 0x01:	/* unknown type / ISDN/Telephony num. plan */
+			break;
+		default:
+			dev_notice(cs->dev,
+				   "%s: %s type/plan 0x%02x unsupported\n",
+				   "CONNECT_REQ", "Calling party number", *pp);
+		}
+		pp++;
+		l--;
+
+		/* check presentation indicator */
+		if (!l) {
+			dev_notice(cs->dev, "%s: %s IE truncated\n",
+				   "CONNECT_REQ", "Calling party number");
+			info = CapiIllMessageParmCoding;
+			goto error;
+		}
+		switch (*pp & 0xfc) { /* ignore Screening indicator */
+		case 0x80:	/* Presentation allowed */
+			s = "^SCLIP=1\r";
+			break;
+		case 0xa0:	/* Presentation restricted */
+			s = "^SCLIP=0\r";
+			break;
+		default:
+			dev_notice(cs->dev, "%s: invalid %s 0x%02x\n",
+				   "CONNECT_REQ",
+				   "Presentation/Screening indicator",
+				   *pp);
+			s = "^SCLIP=1\r";
+		}
+		commands[AT_CLIP] = kstrdup(s, GFP_KERNEL);
+		if (!commands[AT_CLIP])
+			goto oom;
+		pp++;
+		l--;
+
+		if (l) {
+			/* number */
+			commands[AT_MSN] = kmalloc(l+8, GFP_KERNEL);
+			if (!commands[AT_MSN])
+				goto oom;
+			snprintf(commands[AT_MSN], l+8, "^SMSN=%*s\r", l, pp);
+		}
+	}
+
+	/* check parameter: CIP Value */
+	if (cmsg->CIPValue > ARRAY_SIZE(cip2bchlc) ||
+	    (cmsg->CIPValue > 0 && cip2bchlc[cmsg->CIPValue].bc == NULL)) {
+		dev_notice(cs->dev, "%s: unknown CIP value %d\n",
+			   "CONNECT_REQ", cmsg->CIPValue);
+		info = CapiCipValueUnknown;
+		goto error;
+	}
+
+	/* check/encode parameter: BC */
+	if (cmsg->BC && cmsg->BC[0]) {
+		/* explicit BC overrides CIP */
+		l = 2*cmsg->BC[0] + 7;
+		commands[AT_BC] = kmalloc(l, GFP_KERNEL);
+		if (!commands[AT_BC])
+			goto oom;
+		strcpy(commands[AT_BC], "^SBC=");
+		decode_ie(cmsg->BC, commands[AT_BC]+5);
+		strcpy(commands[AT_BC] + l - 2, "\r");
+	} else if (cip2bchlc[cmsg->CIPValue].bc) {
+		l = strlen(cip2bchlc[cmsg->CIPValue].bc) + 7;
+		commands[AT_BC] = kmalloc(l, GFP_KERNEL);
+		if (!commands[AT_BC])
+			goto oom;
+		snprintf(commands[AT_BC], l, "^SBC=%s\r",
+			 cip2bchlc[cmsg->CIPValue].bc);
+	}
+
+	/* check/encode parameter: HLC */
+	if (cmsg->HLC && cmsg->HLC[0]) {
+		/* explicit HLC overrides CIP */
+		l = 2*cmsg->HLC[0] + 7;
+		commands[AT_HLC] = kmalloc(l, GFP_KERNEL);
+		if (!commands[AT_HLC])
+			goto oom;
+		strcpy(commands[AT_HLC], "^SHLC=");
+		decode_ie(cmsg->HLC, commands[AT_HLC]+5);
+		strcpy(commands[AT_HLC] + l - 2, "\r");
+	} else if (cip2bchlc[cmsg->CIPValue].hlc) {
+		l = strlen(cip2bchlc[cmsg->CIPValue].hlc) + 7;
+		commands[AT_HLC] = kmalloc(l, GFP_KERNEL);
+		if (!commands[AT_HLC])
+			goto oom;
+		snprintf(commands[AT_HLC], l, "^SHLC=%s\r",
+			 cip2bchlc[cmsg->CIPValue].hlc);
+	}
+
+	/* check/encode parameter: B Protocol */
+	if (cmsg->BProtocol == CAPI_DEFAULT) {
+		bcs->proto2 = L2_HDLC;
+		dev_warn(cs->dev,
+		    "B2 Protocol X.75 SLP unsupported, using Transparent\n");
+	} else {
+		switch (cmsg->B1protocol) {
+		case 0:
+			bcs->proto2 = L2_HDLC;
+			break;
+		case 1:
+			bcs->proto2 = L2_BITSYNC;
+			break;
+		default:
+			dev_warn(cs->dev,
+			    "B1 Protocol %u unsupported, using Transparent\n",
+				 cmsg->B1protocol);
+			bcs->proto2 = L2_BITSYNC;
+		}
+		if (cmsg->B2protocol != 1)
+			dev_warn(cs->dev,
+			    "B2 Protocol %u unsupported, using Transparent\n",
+				 cmsg->B2protocol);
+		if (cmsg->B3protocol != 0)
+			dev_warn(cs->dev,
+			    "B3 Protocol %u unsupported, using Transparent\n",
+				 cmsg->B3protocol);
+		ignore_cstruct_param(cs, cmsg->B1configuration,
+					"CONNECT_REQ", "B1 Configuration");
+		ignore_cstruct_param(cs, cmsg->B2configuration,
+					"CONNECT_REQ", "B2 Configuration");
+		ignore_cstruct_param(cs, cmsg->B3configuration,
+					"CONNECT_REQ", "B3 Configuration");
+	}
+	commands[AT_PROTO] = kmalloc(9, GFP_KERNEL);
+	if (!commands[AT_PROTO])
+		goto oom;
+	snprintf(commands[AT_PROTO], 9, "^SBPR=%u\r", bcs->proto2);
+
+	/* ToDo: check/encode remaining parameters */
+	ignore_cstruct_param(cs, cmsg->CalledPartySubaddress,
+					"CONNECT_REQ", "Called pty subaddr");
+	ignore_cstruct_param(cs, cmsg->CallingPartySubaddress,
+					"CONNECT_REQ", "Calling pty subaddr");
+	ignore_cstruct_param(cs, cmsg->LLC,
+					"CONNECT_REQ", "LLC");
+	if (cmsg->AdditionalInfo != CAPI_DEFAULT) {
+		ignore_cstruct_param(cs, cmsg->BChannelinformation,
+					"CONNECT_REQ", "B Channel Information");
+		ignore_cstruct_param(cs, cmsg->Keypadfacility,
+					"CONNECT_REQ", "Keypad Facility");
+		ignore_cstruct_param(cs, cmsg->Useruserdata,
+					"CONNECT_REQ", "User-User Data");
+		ignore_cstruct_param(cs, cmsg->Facilitydataarray,
+					"CONNECT_REQ", "Facility Data Array");
+	}
+
+	/* encode parameter: B channel to use */
+	commands[AT_ISO] = kmalloc(9, GFP_KERNEL);
+	if (!commands[AT_ISO])
+		goto oom;
+	snprintf(commands[AT_ISO], 9, "^SISO=%u\r",
+		 (unsigned) bcs->channel + 1);
+
+	/* queue & schedule EV_DIAL event */
+	if (!gigaset_add_event(cs, &bcs->at_state, EV_DIAL, commands,
+			       bcs->at_state.seq_index, NULL))
+		goto oom;
+	gig_dbg(DEBUG_CMD, "scheduling DIAL");
+	gigaset_schedule_event(cs);
+	ap->connected = APCONN_SETUP;
+	send_conf(iif, ap, skb, CapiSuccess);
+	return;
+
+oom:
+	dev_err(cs->dev, "%s: out of memory\n", __func__);
+	info = CAPI_MSGOSRESOURCEERR;
+error:
+	if (commands)
+		for (i = 0; i < AT_NUM; i++)
+			kfree(commands[i]);
+	kfree(commands);
+	gigaset_free_channel(bcs);
+	send_conf(iif, ap, skb, info);
+}
+
+/*
+ * process CONNECT_RESP message
+ * checks protocol parameters and queues an ACCEPT or HUP event
+ */
+static void do_connect_resp(struct gigaset_capi_ctr *iif,
+			    struct gigaset_capi_appl *ap,
+			    struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	_cmsg *cmsg = &iif->acmsg;
+	struct bc_state *bcs;
+	struct gigaset_capi_appl *oap;
+	int channel;
+
+	/* decode message */
+	capi_message2cmsg(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+	dev_kfree_skb_any(skb);
+
+	/* extract and check channel number from PLCI */
+	channel = (cmsg->adr.adrPLCI >> 8) & 0xff;
+	if (!channel || channel > cs->channels) {
+		dev_notice(cs->dev, "%s: invalid %s 0x%02x\n",
+			   "CONNECT_RESP", "PLCI", cmsg->adr.adrPLCI);
+		return;
+	}
+	bcs = cs->bcs + channel - 1;
+
+	switch (cmsg->Reject) {
+	case 0:		/* Accept */
+		/* drop all competing applications, keep only this one */
+		for (oap = bcs->ap; oap != NULL; oap = oap->bcnext)
+			if (oap != ap)
+				send_disconnect_ind(bcs, oap,
+					CapiCallGivenToOtherApplication);
+		ap->bcnext = NULL;
+		bcs->ap = ap;
+		bcs->chstate |= CHS_NOTIFY_LL;
+
+		/* check/encode B channel protocol */
+		if (cmsg->BProtocol == CAPI_DEFAULT) {
+			bcs->proto2 = L2_HDLC;
+			dev_warn(cs->dev,
+		"B2 Protocol X.75 SLP unsupported, using Transparent\n");
+		} else {
+			switch (cmsg->B1protocol) {
+			case 0:
+				bcs->proto2 = L2_HDLC;
+				break;
+			case 1:
+				bcs->proto2 = L2_BITSYNC;
+				break;
+			default:
+				dev_warn(cs->dev,
+			"B1 Protocol %u unsupported, using Transparent\n",
+					 cmsg->B1protocol);
+				bcs->proto2 = L2_BITSYNC;
+			}
+			if (cmsg->B2protocol != 1)
+				dev_warn(cs->dev,
+			"B2 Protocol %u unsupported, using Transparent\n",
+					 cmsg->B2protocol);
+			if (cmsg->B3protocol != 0)
+				dev_warn(cs->dev,
+			"B3 Protocol %u unsupported, using Transparent\n",
+					 cmsg->B3protocol);
+			ignore_cstruct_param(cs, cmsg->B1configuration,
+					"CONNECT_RESP", "B1 Configuration");
+			ignore_cstruct_param(cs, cmsg->B2configuration,
+					"CONNECT_RESP", "B2 Configuration");
+			ignore_cstruct_param(cs, cmsg->B3configuration,
+					"CONNECT_RESP", "B3 Configuration");
+		}
+
+		/* ToDo: check/encode remaining parameters */
+		ignore_cstruct_param(cs, cmsg->ConnectedNumber,
+					"CONNECT_RESP", "Connected Number");
+		ignore_cstruct_param(cs, cmsg->ConnectedSubaddress,
+					"CONNECT_RESP", "Connected Subaddress");
+		ignore_cstruct_param(cs, cmsg->LLC,
+					"CONNECT_RESP", "LLC");
+		if (cmsg->AdditionalInfo != CAPI_DEFAULT) {
+			ignore_cstruct_param(cs, cmsg->BChannelinformation,
+					"CONNECT_RESP", "BChannel Information");
+			ignore_cstruct_param(cs, cmsg->Keypadfacility,
+					"CONNECT_RESP", "Keypad Facility");
+			ignore_cstruct_param(cs, cmsg->Useruserdata,
+					"CONNECT_RESP", "User-User Data");
+			ignore_cstruct_param(cs, cmsg->Facilitydataarray,
+					"CONNECT_RESP", "Facility Data Array");
+		}
+
+		/* Accept call */
+		if (!gigaset_add_event(cs, &cs->bcs[channel-1].at_state,
+				       EV_ACCEPT, NULL, 0, NULL))
+			return;
+		gig_dbg(DEBUG_CMD, "scheduling ACCEPT");
+		gigaset_schedule_event(cs);
+		return;
+
+	case 1:			/* Ignore */
+		/* send DISCONNECT_IND to this application */
+		send_disconnect_ind(bcs, ap, 0);
+
+		/* remove it from the list of listening apps */
+		if (bcs->ap == ap) {
+			bcs->ap = ap->bcnext;
+			if (bcs->ap == NULL)
+				/* last one: stop ev-layer hupD notifications */
+				bcs->chstate &= ~CHS_NOTIFY_LL;
+			return;
+		}
+		for (oap = bcs->ap; oap != NULL; oap = oap->bcnext) {
+			if (oap->bcnext == ap) {
+				oap->bcnext = oap->bcnext->bcnext;
+				return;
+			}
+		}
+		dev_err(cs->dev, "%s: application %u not found\n",
+			__func__, ap->id);
+		return;
+
+	default:		/* Reject */
+		/* drop all competing applications, keep only this one */
+		for (oap = bcs->ap; oap != NULL; oap = oap->bcnext)
+			if (oap != ap)
+				send_disconnect_ind(bcs, oap,
+					CapiCallGivenToOtherApplication);
+		ap->bcnext = NULL;
+		bcs->ap = ap;
+
+		/* reject call - will trigger DISCONNECT_IND for this app */
+		dev_info(cs->dev, "%s: Reject=%x\n",
+			 "CONNECT_RESP", cmsg->Reject);
+		if (!gigaset_add_event(cs, &cs->bcs[channel-1].at_state,
+				       EV_HUP, NULL, 0, NULL))
+			return;
+		gig_dbg(DEBUG_CMD, "scheduling HUP");
+		gigaset_schedule_event(cs);
+		return;
+	}
+}
+
+/*
+ * process CONNECT_B3_REQ message
+ * build NCCI and emit CONNECT_B3_CONF reply
+ */
+static void do_connect_b3_req(struct gigaset_capi_ctr *iif,
+			      struct gigaset_capi_appl *ap,
+			      struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	_cmsg *cmsg = &iif->acmsg;
+	int channel;
+
+	/* decode message */
+	capi_message2cmsg(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+
+	/* extract and check channel number from PLCI */
+	channel = (cmsg->adr.adrPLCI >> 8) & 0xff;
+	if (!channel || channel > cs->channels) {
+		dev_notice(cs->dev, "%s: invalid %s 0x%02x\n",
+			   "CONNECT_B3_REQ", "PLCI", cmsg->adr.adrPLCI);
+		send_conf(iif, ap, skb, CapiIllContrPlciNcci);
+		return;
+	}
+
+	/* mark logical connection active */
+	ap->connected = APCONN_ACTIVE;
+
+	/* build NCCI: always 1 (one B3 connection only) */
+	cmsg->adr.adrNCCI |= 1 << 16;
+
+	/* NCPI parameter: not applicable for B3 Transparent */
+	ignore_cstruct_param(cs, cmsg->NCPI, "CONNECT_B3_REQ", "NCPI");
+	send_conf(iif, ap, skb, (cmsg->NCPI && cmsg->NCPI[0]) ?
+				CapiNcpiNotSupportedByProtocol : CapiSuccess);
+}
+
+/*
+ * process CONNECT_B3_RESP message
+ * Depending on the Reject parameter, either emit CONNECT_B3_ACTIVE_IND
+ * or queue EV_HUP and emit DISCONNECT_B3_IND.
+ * The emitted message is always shorter than the received one,
+ * allowing to reuse the skb.
+ */
+static void do_connect_b3_resp(struct gigaset_capi_ctr *iif,
+			       struct gigaset_capi_appl *ap,
+			       struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	_cmsg *cmsg = &iif->acmsg;
+	struct bc_state *bcs;
+	int channel;
+	unsigned int msgsize;
+	u8 command;
+
+	/* decode message */
+	capi_message2cmsg(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+
+	/* extract and check channel number and NCCI */
+	channel = (cmsg->adr.adrNCCI >> 8) & 0xff;
+	if (!channel || channel > cs->channels ||
+	    ((cmsg->adr.adrNCCI >> 16) & 0xffff) != 1) {
+		dev_notice(cs->dev, "%s: invalid %s 0x%02x\n",
+			   "CONNECT_B3_RESP", "NCCI", cmsg->adr.adrNCCI);
+		dev_kfree_skb_any(skb);
+		return;
+	}
+	bcs = &cs->bcs[channel-1];
+
+	if (cmsg->Reject) {
+		/* Reject: clear B3 connect received flag */
+		ap->connected = APCONN_SETUP;
+
+		/* trigger hangup, causing eventual DISCONNECT_IND */
+		if (!gigaset_add_event(cs, &bcs->at_state,
+				       EV_HUP, NULL, 0, NULL)) {
+			dev_err(cs->dev, "%s: out of memory\n", __func__);
+			dev_kfree_skb_any(skb);
+			return;
+		}
+		gig_dbg(DEBUG_CMD, "scheduling HUP");
+		gigaset_schedule_event(cs);
+
+		/* emit DISCONNECT_B3_IND */
+		command = CAPI_DISCONNECT_B3;
+		msgsize = CAPI_DISCONNECT_B3_IND_BASELEN;
+	} else {
+		/*
+		 * Accept: emit CONNECT_B3_ACTIVE_IND immediately, as
+		 * we only send CONNECT_B3_IND if the B channel is up
+		 */
+		command = CAPI_CONNECT_B3_ACTIVE;
+		msgsize = CAPI_CONNECT_B3_ACTIVE_IND_BASELEN;
+	}
+	capi_cmsg_header(cmsg, ap->id, command, CAPI_IND,
+			 ap->nextMessageNumber++, cmsg->adr.adrNCCI);
+	__skb_trim(skb, msgsize);
+	capi_cmsg2message(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+	capi_ctr_handle_message(&iif->ctr, ap->id, skb);
+}
+
+/*
+ * process DISCONNECT_REQ message
+ * schedule EV_HUP and emit DISCONNECT_B3_IND if necessary,
+ * emit DISCONNECT_CONF reply
+ */
+static void do_disconnect_req(struct gigaset_capi_ctr *iif,
+			      struct gigaset_capi_appl *ap,
+			      struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	_cmsg *cmsg = &iif->acmsg;
+	struct bc_state *bcs;
+	_cmsg *b3cmsg;
+	struct sk_buff *b3skb;
+	int channel;
+
+	/* decode message */
+	capi_message2cmsg(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+
+	/* extract and check channel number from PLCI */
+	channel = (cmsg->adr.adrPLCI >> 8) & 0xff;
+	if (!channel || channel > cs->channels) {
+		dev_notice(cs->dev, "%s: invalid %s 0x%02x\n",
+			   "DISCONNECT_REQ", "PLCI", cmsg->adr.adrPLCI);
+		send_conf(iif, ap, skb, CapiIllContrPlciNcci);
+		return;
+	}
+	bcs = cs->bcs + channel - 1;
+
+	/* ToDo: process parameter: Additional info */
+	if (cmsg->AdditionalInfo != CAPI_DEFAULT) {
+		ignore_cstruct_param(cs, cmsg->BChannelinformation,
+				     "DISCONNECT_REQ", "B Channel Information");
+		ignore_cstruct_param(cs, cmsg->Keypadfacility,
+				     "DISCONNECT_REQ", "Keypad Facility");
+		ignore_cstruct_param(cs, cmsg->Useruserdata,
+				     "DISCONNECT_REQ", "User-User Data");
+		ignore_cstruct_param(cs, cmsg->Facilitydataarray,
+				     "DISCONNECT_REQ", "Facility Data Array");
+	}
+
+	/* skip if DISCONNECT_IND already sent */
+	if (!ap->connected)
+		return;
+
+	/* check for active logical connection */
+	if (ap->connected >= APCONN_ACTIVE) {
+		/*
+		 * emit DISCONNECT_B3_IND with cause 0x3301
+		 * use separate cmsg structure, as the content of iif->acmsg
+		 * is still needed for creating the _CONF message
+		 */
+		b3cmsg = kmalloc(sizeof(*b3cmsg), GFP_KERNEL);
+		if (!b3cmsg) {
+			dev_err(cs->dev, "%s: out of memory\n", __func__);
+			send_conf(iif, ap, skb, CAPI_MSGOSRESOURCEERR);
+			return;
+		}
+		capi_cmsg_header(b3cmsg, ap->id, CAPI_DISCONNECT_B3, CAPI_IND,
+				 ap->nextMessageNumber++,
+				 cmsg->adr.adrPLCI | (1 << 16));
+		b3cmsg->Reason_B3 = CapiProtocolErrorLayer1;
+		b3skb = alloc_skb(CAPI_DISCONNECT_B3_IND_BASELEN, GFP_KERNEL);
+		if (b3skb == NULL) {
+			dev_err(cs->dev, "%s: out of memory\n", __func__);
+			send_conf(iif, ap, skb, CAPI_MSGOSRESOURCEERR);
+			return;
+		}
+		capi_cmsg2message(b3cmsg,
+			__skb_put(b3skb, CAPI_DISCONNECT_B3_IND_BASELEN));
+		kfree(b3cmsg);
+		capi_ctr_handle_message(&iif->ctr, ap->id, b3skb);
+	}
+
+	/* trigger hangup, causing eventual DISCONNECT_IND */
+	if (!gigaset_add_event(cs, &bcs->at_state, EV_HUP, NULL, 0, NULL)) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		send_conf(iif, ap, skb, CAPI_MSGOSRESOURCEERR);
+		return;
+	}
+	gig_dbg(DEBUG_CMD, "scheduling HUP");
+	gigaset_schedule_event(cs);
+
+	/* emit reply */
+	send_conf(iif, ap, skb, CapiSuccess);
+}
+
+/*
+ * process DISCONNECT_B3_REQ message
+ * schedule EV_HUP and emit DISCONNECT_B3_CONF reply
+ */
+static void do_disconnect_b3_req(struct gigaset_capi_ctr *iif,
+				 struct gigaset_capi_appl *ap,
+				 struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	_cmsg *cmsg = &iif->acmsg;
+	int channel;
+
+	/* decode message */
+	capi_message2cmsg(cmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, cmsg);
+
+	/* extract and check channel number and NCCI */
+	channel = (cmsg->adr.adrNCCI >> 8) & 0xff;
+	if (!channel || channel > cs->channels ||
+	    ((cmsg->adr.adrNCCI >> 16) & 0xffff) != 1) {
+		dev_notice(cs->dev, "%s: invalid %s 0x%02x\n",
+			   "DISCONNECT_B3_REQ", "NCCI", cmsg->adr.adrNCCI);
+		send_conf(iif, ap, skb, CapiIllContrPlciNcci);
+		return;
+	}
+
+	/* reject if logical connection not active */
+	if (ap->connected < APCONN_ACTIVE) {
+		send_conf(iif, ap, skb,
+			  CapiMessageNotSupportedInCurrentState);
+		return;
+	}
+
+	/* trigger hangup, causing eventual DISCONNECT_B3_IND */
+	if (!gigaset_add_event(cs, &cs->bcs[channel-1].at_state,
+			       EV_HUP, NULL, 0, NULL)) {
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
+		send_conf(iif, ap, skb, CAPI_MSGOSRESOURCEERR);
+		return;
+	}
+	gig_dbg(DEBUG_CMD, "scheduling HUP");
+	gigaset_schedule_event(cs);
+
+	/* NCPI parameter: not applicable for B3 Transparent */
+	ignore_cstruct_param(cs, cmsg->NCPI,
+				"DISCONNECT_B3_REQ", "NCPI");
+	send_conf(iif, ap, skb, (cmsg->NCPI && cmsg->NCPI[0]) ?
+				CapiNcpiNotSupportedByProtocol : CapiSuccess);
+}
+
+/*
+ * process DATA_B3_REQ message
+ */
+static void do_data_b3_req(struct gigaset_capi_ctr *iif,
+			   struct gigaset_capi_appl *ap,
+			   struct sk_buff *skb)
+{
+	struct cardstate *cs = iif->ctr.driverdata;
+	int channel = CAPIMSG_PLCI_PART(skb->data);
+	u16 ncci = CAPIMSG_NCCI_PART(skb->data);
+	u16 msglen = CAPIMSG_LEN(skb->data);
+	u16 datalen = CAPIMSG_DATALEN(skb->data);
+	u16 flags = CAPIMSG_FLAGS(skb->data);
+
+	/* frequent message, avoid _cmsg overhead */
+	dump_rawmsg(DEBUG_LLDATA, "DATA_B3_REQ", skb->data);
+
+	gig_dbg(DEBUG_LLDATA,
+		"Receiving data from LL (ch: %d, flg: %x, sz: %d|%d)",
+		channel, flags, msglen, datalen);
+
+	/* check parameters */
+	if (channel == 0 || channel > cs->channels || ncci != 1) {
+		dev_notice(cs->dev, "%s: invalid %s 0x%02x\n",
+			   "DATA_B3_REQ", "NCCI", CAPIMSG_NCCI(skb->data));
+		send_conf(iif, ap, skb, CapiIllContrPlciNcci);
+		return;
+	}
+	if (msglen != CAPI_DATA_B3_REQ_LEN && msglen != CAPI_DATA_B3_REQ_LEN64)
+		dev_notice(cs->dev, "%s: unexpected length %d\n",
+			   "DATA_B3_REQ", msglen);
+	if (msglen + datalen != skb->len)
+		dev_notice(cs->dev, "%s: length mismatch (%d+%d!=%d)\n",
+			   "DATA_B3_REQ", msglen, datalen, skb->len);
+	if (msglen + datalen > skb->len) {
+		/* message too short for announced data length */
+		send_conf(iif, ap, skb, CapiIllMessageParmCoding); /* ? */
+		return;
+	}
+	if (flags & CAPI_FLAGS_RESERVED) {
+		dev_notice(cs->dev, "%s: reserved flags set (%x)\n",
+			   "DATA_B3_REQ", flags);
+		send_conf(iif, ap, skb, CapiIllMessageParmCoding);
+		return;
+	}
+
+	/* reject if logical connection not active */
+	if (ap->connected < APCONN_ACTIVE) {
+		send_conf(iif, ap, skb, CapiMessageNotSupportedInCurrentState);
+		return;
+	}
+
+	/* pull CAPI message into link layer header */
+	skb_reset_mac_header(skb);
+	skb->mac_len = msglen;
+	skb_pull(skb, msglen);
+
+	/* pass to device-specific module */
+	if (cs->ops->send_skb(&cs->bcs[channel-1], skb) < 0) {
+		send_conf(iif, ap, skb, CAPI_MSGOSRESOURCEERR);
+		return;
+	}
+
+	/* DATA_B3_CONF reply will be sent by gigaset_skb_sent() */
+
+	/*
+	 * ToDo: honor unset "delivery confirmation" bit
+	 * (send DATA_B3_CONF immediately?)
+	 */
+}
+
+/*
+ * process RESET_B3_REQ message
+ * just always reply "not supported by current protocol"
+ */
+static void do_reset_b3_req(struct gigaset_capi_ctr *iif,
+			    struct gigaset_capi_appl *ap,
+			    struct sk_buff *skb)
+{
+	/* decode message */
+	capi_message2cmsg(&iif->acmsg, skb->data);
+	dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
+	send_conf(iif, ap, skb,
+		  CapiResetProcedureNotSupportedByCurrentProtocol);
+}
+
+/*
+ * dump unsupported/ignored messages at most twice per minute,
+ * some apps send those very frequently
+ */
+static unsigned long ignored_msg_dump_time;
+
+/*
+ * unsupported CAPI message handler
+ */
+static void do_unsupported(struct gigaset_capi_ctr *iif,
+			   struct gigaset_capi_appl *ap,
+			   struct sk_buff *skb)
+{
+	/* decode message */
+	capi_message2cmsg(&iif->acmsg, skb->data);
+	if (printk_timed_ratelimit(&ignored_msg_dump_time, 30 * 1000))
+		dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
+	send_conf(iif, ap, skb, CapiMessageNotSupportedInCurrentState);
+}
+
+/*
+ * CAPI message handler: no-op
+ */
+static void do_nothing(struct gigaset_capi_ctr *iif,
+		       struct gigaset_capi_appl *ap,
+		       struct sk_buff *skb)
+{
+	if (printk_timed_ratelimit(&ignored_msg_dump_time, 30 * 1000)) {
+		/* decode message */
+		capi_message2cmsg(&iif->acmsg, skb->data);
+		dump_cmsg(DEBUG_CMD, __func__, &iif->acmsg);
+	}
+	dev_kfree_skb_any(skb);
+}
+
+static void do_data_b3_resp(struct gigaset_capi_ctr *iif,
+			    struct gigaset_capi_appl *ap,
+			    struct sk_buff *skb)
+{
+	dump_rawmsg(DEBUG_LLDATA, __func__, skb->data);
+	dev_kfree_skb_any(skb);
+}
+
+/* table of outgoing CAPI message handlers with lookup function */
+typedef void (*capi_send_handler_t)(struct gigaset_capi_ctr *,
+				    struct gigaset_capi_appl *,
+				    struct sk_buff *);
+
+static struct {
+	u16 cmd;
+	capi_send_handler_t handler;
+} capi_send_handler_table[] = {
+	/* most frequent messages first for faster lookup */
+	{ CAPI_DATA_B3_REQ, do_data_b3_req },
+	{ CAPI_DATA_B3_RESP, do_data_b3_resp },
+
+	{ CAPI_ALERT_REQ, do_alert_req },
+	{ CAPI_CONNECT_ACTIVE_RESP, do_nothing },
+	{ CAPI_CONNECT_B3_ACTIVE_RESP, do_nothing },
+	{ CAPI_CONNECT_B3_REQ, do_connect_b3_req },
+	{ CAPI_CONNECT_B3_RESP, do_connect_b3_resp },
+	{ CAPI_CONNECT_B3_T90_ACTIVE_RESP, do_nothing },
+	{ CAPI_CONNECT_REQ, do_connect_req },
+	{ CAPI_CONNECT_RESP, do_connect_resp },
+	{ CAPI_DISCONNECT_B3_REQ, do_disconnect_b3_req },
+	{ CAPI_DISCONNECT_B3_RESP, do_nothing },
+	{ CAPI_DISCONNECT_REQ, do_disconnect_req },
+	{ CAPI_DISCONNECT_RESP, do_nothing },
+	{ CAPI_FACILITY_REQ, do_facility_req },
+	{ CAPI_FACILITY_RESP, do_nothing },
+	{ CAPI_LISTEN_REQ, do_listen_req },
+	{ CAPI_SELECT_B_PROTOCOL_REQ, do_unsupported },
+	{ CAPI_RESET_B3_REQ, do_reset_b3_req },
+	{ CAPI_RESET_B3_RESP, do_nothing },
+
+	/*
+	 * ToDo: support overlap sending (requires ev-layer state
+	 * machine extension to generate additional ATD commands)
+	 */
+	{ CAPI_INFO_REQ, do_unsupported },
+	{ CAPI_INFO_RESP, do_nothing },
+
+	/*
+	 * ToDo: what's the proper response for these?
+	 */
+	{ CAPI_MANUFACTURER_REQ, do_nothing },
+	{ CAPI_MANUFACTURER_RESP, do_nothing },
+};
+
+/* look up handler */
+static inline capi_send_handler_t lookup_capi_send_handler(const u16 cmd)
+{
+	size_t i;
+
+	for (i = 0; i < ARRAY_SIZE(capi_send_handler_table); i++)
+		if (capi_send_handler_table[i].cmd == cmd)
+			return capi_send_handler_table[i].handler;
+	return NULL;
+}
+
+
+/**
+ * gigaset_send_message() - accept a CAPI message from an application
+ * @ctr:	controller descriptor structure.
+ * @skb:	CAPI message.
+ *
+ * Return value: CAPI error code
+ * Note: capidrv (and probably others, too) only uses the return value to
+ * decide whether it has to free the skb (only if result != CAPI_NOERROR (0))
+ */
+static u16 gigaset_send_message(struct capi_ctr *ctr, struct sk_buff *skb)
+{
+	struct gigaset_capi_ctr *iif
+		= container_of(ctr, struct gigaset_capi_ctr, ctr);
+	struct cardstate *cs = ctr->driverdata;
+	struct gigaset_capi_appl *ap;
+	capi_send_handler_t handler;
+
+	/* can only handle linear sk_buffs */
+	if (skb_linearize(skb) < 0) {
+		dev_warn(cs->dev, "%s: skb_linearize failed\n", __func__);
+		return CAPI_MSGOSRESOURCEERR;
+	}
+
+	/* retrieve application data structure */
+	ap = get_appl(iif, CAPIMSG_APPID(skb->data));
+	if (!ap) {
+		dev_notice(cs->dev, "%s: application %u not registered\n",
+			   __func__, CAPIMSG_APPID(skb->data));
+		return CAPI_ILLAPPNR;
+	}
+
+	/* look up command */
+	handler = lookup_capi_send_handler(CAPIMSG_CMD(skb->data));
+	if (!handler) {
+		/* unknown/unsupported message type */
+		if (printk_ratelimit())
+			dev_notice(cs->dev, "%s: unsupported message %u\n",
+				   __func__, CAPIMSG_CMD(skb->data));
+		return CAPI_ILLCMDORSUBCMDORMSGTOSMALL;
+	}
+
+	/* serialize */
+	if (atomic_add_return(1, &iif->sendqlen) > 1) {
+		/* queue behind other messages */
+		skb_queue_tail(&iif->sendqueue, skb);
+		return CAPI_NOERROR;
+	}
+
+	/* process message */
+	handler(iif, ap, skb);
+
+	/* process other messages arrived in the meantime */
+	while (atomic_sub_return(1, &iif->sendqlen) > 0) {
+		skb = skb_dequeue(&iif->sendqueue);
+		if (!skb) {
+			/* should never happen */
+			dev_err(cs->dev, "%s: send queue empty\n", __func__);
+			continue;
+		}
+		ap = get_appl(iif, CAPIMSG_APPID(skb->data));
+		if (!ap) {
+			/* could that happen? */
+			dev_warn(cs->dev, "%s: application %u vanished\n",
+				 __func__, CAPIMSG_APPID(skb->data));
+			continue;
+		}
+		handler = lookup_capi_send_handler(CAPIMSG_CMD(skb->data));
+		if (!handler) {
+			/* should never happen */
+			dev_err(cs->dev, "%s: handler %x vanished\n",
+				__func__, CAPIMSG_CMD(skb->data));
+			continue;
+		}
+		handler(iif, ap, skb);
+	}
+
+	return CAPI_NOERROR;
+}
+
+/**
+ * gigaset_procinfo() - build single line description for controller
+ * @ctr:	controller descriptor structure.
+ *
+ * Return value: pointer to generated string (null terminated)
+ */
+static char *gigaset_procinfo(struct capi_ctr *ctr)
+{
+	return ctr->name;	/* ToDo: more? */
+}
+
+/**
+ * gigaset_ctr_read_proc() - build controller proc file entry
+ * @page:	buffer of PAGE_SIZE bytes for receiving the entry.
+ * @start:	unused.
+ * @off:	unused.
+ * @count:	unused.
+ * @eof:	unused.
+ * @ctr:	controller descriptor structure.
+ *
+ * Return value: length of generated entry
+ */
+static int gigaset_ctr_read_proc(char *page, char **start, off_t off,
+			  int count, int *eof, struct capi_ctr *ctr)
+{
+	struct cardstate *cs = ctr->driverdata;
+	char *s;
+	int i;
+	int len = 0;
+	len += sprintf(page+len, "%-16s %s\n", "name", ctr->name);
+	len += sprintf(page+len, "%-16s %s %s\n", "dev",
+			dev_driver_string(cs->dev), dev_name(cs->dev));
+	len += sprintf(page+len, "%-16s %d\n", "id", cs->myid);
+	if (cs->gotfwver)
+		len += sprintf(page+len, "%-16s %d.%d.%d.%d\n", "firmware",
+			cs->fwver[0], cs->fwver[1], cs->fwver[2], cs->fwver[3]);
+	len += sprintf(page+len, "%-16s %d\n", "channels",
+			cs->channels);
+	len += sprintf(page+len, "%-16s %s\n", "onechannel",
+			cs->onechannel ? "yes" : "no");
+
+	switch (cs->mode) {
+	case M_UNKNOWN:
+		s = "unknown";
+		break;
+	case M_CONFIG:
+		s = "config";
+		break;
+	case M_UNIMODEM:
+		s = "Unimodem";
+		break;
+	case M_CID:
+		s = "CID";
+		break;
+	default:
+		s = "??";
+	}
+	len += sprintf(page+len, "%-16s %s\n", "mode", s);
+
+	switch (cs->mstate) {
+	case MS_UNINITIALIZED:
+		s = "uninitialized";
+		break;
+	case MS_INIT:
+		s = "init";
+		break;
+	case MS_LOCKED:
+		s = "locked";
+		break;
+	case MS_SHUTDOWN:
+		s = "shutdown";
+		break;
+	case MS_RECOVER:
+		s = "recover";
+		break;
+	case MS_READY:
+		s = "ready";
+		break;
+	default:
+		s = "??";
+	}
+	len += sprintf(page+len, "%-16s %s\n", "mstate", s);
+
+	len += sprintf(page+len, "%-16s %s\n", "running",
+			cs->running ? "yes" : "no");
+	len += sprintf(page+len, "%-16s %s\n", "connected",
+			cs->connected ? "yes" : "no");
+	len += sprintf(page+len, "%-16s %s\n", "isdn_up",
+			cs->isdn_up ? "yes" : "no");
+	len += sprintf(page+len, "%-16s %s\n", "cidmode",
+			cs->cidmode ? "yes" : "no");
+
+	for (i = 0; i < cs->channels; i++) {
+		len += sprintf(page+len, "[%d]%-13s %d\n", i, "corrupted",
+				cs->bcs[i].corrupted);
+		len += sprintf(page+len, "[%d]%-13s %d\n", i, "trans_down",
+				cs->bcs[i].trans_down);
+		len += sprintf(page+len, "[%d]%-13s %d\n", i, "trans_up",
+				cs->bcs[i].trans_up);
+		len += sprintf(page+len, "[%d]%-13s %d\n", i, "chstate",
+				cs->bcs[i].chstate);
+		switch (cs->bcs[i].proto2) {
+		case L2_BITSYNC:
+			s = "bitsync";
+			break;
+		case L2_HDLC:
+			s = "HDLC";
+			break;
+		case L2_VOICE:
+			s = "voice";
+			break;
+		default:
+			s = "??";
+		}
+		len += sprintf(page+len, "[%d]%-13s %s\n", i, "proto2", s);
+	}
+	return len;
+}
+
+
+static struct capi_driver capi_driver_gigaset = {
+	.name		= "gigaset",
+	.revision	= "1.0",
+};
+
+/**
+ * gigaset_isdn_register() - register to LL
+ * @cs:		device descriptor structure.
+ * @isdnid:	device name.
+ *
+ * Called by main module to register the device with the LL.
+ *
+ * Return value: 1 for success, 0 for failure
+ */
+int gigaset_isdn_register(struct cardstate *cs, const char *isdnid)
+{
+	struct gigaset_capi_ctr *iif;
+	int rc;
+
+	pr_info("Kernel CAPI interface\n");
+
+	iif = kmalloc(sizeof(*iif), GFP_KERNEL);
+	if (!iif) {
+		pr_err("%s: out of memory\n", __func__);
+		return 0;
+	}
+
+	/* register driver with CAPI (ToDo: what for?) */
+	register_capi_driver(&capi_driver_gigaset);
+
+	/* prepare controller structure */
+	iif->ctr.owner         = THIS_MODULE;
+	iif->ctr.driverdata    = cs;
+	strncpy(iif->ctr.name, isdnid, sizeof(iif->ctr.name));
+	iif->ctr.driver_name   = "gigaset";
+	iif->ctr.load_firmware = gigaset_load_firmware;
+	iif->ctr.reset_ctr     = gigaset_reset_ctr;
+	iif->ctr.register_appl = gigaset_register_appl;
+	iif->ctr.release_appl  = gigaset_release_appl;
+	iif->ctr.send_message  = gigaset_send_message;
+	iif->ctr.procinfo      = gigaset_procinfo;
+	iif->ctr.ctr_read_proc = gigaset_ctr_read_proc;
+	INIT_LIST_HEAD(&iif->appls);
+	skb_queue_head_init(&iif->sendqueue);
+	atomic_set(&iif->sendqlen, 0);
+
+	/* register controller with CAPI */
+	rc = attach_capi_ctr(&iif->ctr);
+	if (rc) {
+		pr_err("attach_capi_ctr failed (%d)\n", rc);
+		unregister_capi_driver(&capi_driver_gigaset);
+		kfree(iif);
+		return 0;
+	}
+
+	cs->iif = iif;
+	cs->hw_hdr_len = CAPI_DATA_B3_REQ_LEN;
+	return 1;
+}
+
+/**
+ * gigaset_isdn_unregister() - unregister from LL
+ * @cs:		device descriptor structure.
+ *
+ * Called by main module to unregister the device from the LL.
+ */
+void gigaset_isdn_unregister(struct cardstate *cs)
+{
+	struct gigaset_capi_ctr *iif = cs->iif;
+
+	detach_capi_ctr(&iif->ctr);
+	kfree(iif);
+	cs->iif = NULL;
+	unregister_capi_driver(&capi_driver_gigaset);
+}
diff --git a/drivers/isdn/gigaset/common.c b/drivers/isdn/gigaset/common.c
index 33dcd8d72b7c..82ed1cd14ff5 100644
--- a/drivers/isdn/gigaset/common.c
+++ b/drivers/isdn/gigaset/common.c
@@ -108,7 +108,7 @@ int gigaset_enterconfigmode(struct cardstate *cs)
 {
 	int i, r;
 
-	cs->control_state = TIOCM_RTS; //FIXME
+	cs->control_state = TIOCM_RTS;
 
 	r = setflags(cs, TIOCM_DTR, 200);
 	if (r < 0)
@@ -132,10 +132,10 @@ int gigaset_enterconfigmode(struct cardstate *cs)
 
 error:
 	dev_err(cs->dev, "error %d on setuartbits\n", -r);
-	cs->control_state = TIOCM_RTS|TIOCM_DTR; // FIXME is this a good value?
+	cs->control_state = TIOCM_RTS|TIOCM_DTR;
 	cs->ops->set_modem_ctrl(cs, 0, TIOCM_RTS|TIOCM_DTR);
 
-	return -1; //r
+	return -1;
 }
 
 static int test_timeout(struct at_state_t *at_state)
@@ -150,10 +150,9 @@ static int test_timeout(struct at_state_t *at_state)
 	}
 
 	if (!gigaset_add_event(at_state->cs, at_state, EV_TIMEOUT, NULL,
-			       at_state->timer_index, NULL)) {
-		//FIXME what should we do?
-	}
-
+			       at_state->timer_index, NULL))
+			dev_err(at_state->cs->dev, "%s: out of memory\n",
+				__func__);
 	return 1;
 }
 
@@ -207,6 +206,32 @@ int gigaset_get_channel(struct bc_state *bcs)
 	return 1;
 }
 
+struct bc_state *gigaset_get_free_channel(struct cardstate *cs)
+{
+	unsigned long flags;
+	int i;
+
+	spin_lock_irqsave(&cs->lock, flags);
+	if (!try_module_get(cs->driver->owner)) {
+		gig_dbg(DEBUG_ANY,
+			"could not get module for allocating channel");
+		spin_unlock_irqrestore(&cs->lock, flags);
+		return NULL;
+	}
+	for (i = 0; i < cs->channels; ++i)
+		if (!cs->bcs[i].use_count) {
+			++cs->bcs[i].use_count;
+			cs->bcs[i].busy = 1;
+			spin_unlock_irqrestore(&cs->lock, flags);
+			gig_dbg(DEBUG_ANY, "allocated channel %d", i);
+			return cs->bcs + i;
+		}
+	module_put(cs->driver->owner);
+	spin_unlock_irqrestore(&cs->lock, flags);
+	gig_dbg(DEBUG_ANY, "no free channel");
+	return NULL;
+}
+
 void gigaset_free_channel(struct bc_state *bcs)
 {
 	unsigned long flags;
@@ -367,16 +392,15 @@ static void gigaset_freebcs(struct bc_state *bcs)
 	int i;
 
 	gig_dbg(DEBUG_INIT, "freeing bcs[%d]->hw", bcs->channel);
-	if (!bcs->cs->ops->freebcshw(bcs)) {
+	if (!bcs->cs->ops->freebcshw(bcs))
 		gig_dbg(DEBUG_INIT, "failed");
-	}
 
 	gig_dbg(DEBUG_INIT, "clearing bcs[%d]->at_state", bcs->channel);
 	clear_at_state(&bcs->at_state);
 	gig_dbg(DEBUG_INIT, "freeing bcs[%d]->skb", bcs->channel);
+	dev_kfree_skb(bcs->skb);
+	bcs->skb = NULL;
 
-	if (bcs->skb)
-		dev_kfree_skb(bcs->skb);
 	for (i = 0; i < AT_NUM; ++i) {
 		kfree(bcs->commands[i]);
 		bcs->commands[i] = NULL;
@@ -463,6 +487,12 @@ void gigaset_freecs(struct cardstate *cs)
 
 	switch (cs->cs_init) {
 	default:
+		/* clear B channel structures */
+		for (i = 0; i < cs->channels; ++i) {
+			gig_dbg(DEBUG_INIT, "clearing bcs[%d]", i);
+			gigaset_freebcs(cs->bcs + i);
+		}
+
 		/* clear device sysfs */
 		gigaset_free_dev_sysfs(cs);
 
@@ -471,28 +501,20 @@ void gigaset_freecs(struct cardstate *cs)
 		gig_dbg(DEBUG_INIT, "clearing hw");
 		cs->ops->freecshw(cs);
 
-		//FIXME cmdbuf
-
 		/* fall through */
 	case 2: /* error in initcshw */
 		/* Deregister from LL */
 		make_invalid(cs, VALID_ID);
-		gig_dbg(DEBUG_INIT, "clearing iif");
-		gigaset_i4l_cmd(cs, ISDN_STAT_UNLOAD);
+		gigaset_isdn_unregister(cs);
 
 		/* fall through */
-	case 1: /* error when regestering to LL */
+	case 1: /* error when registering to LL */
 		gig_dbg(DEBUG_INIT, "clearing at_state");
 		clear_at_state(&cs->at_state);
 		dealloc_at_states(cs);
 
 		/* fall through */
-	case 0: /* error in one call to initbcs */
-		for (i = 0; i < cs->channels; ++i) {
-			gig_dbg(DEBUG_INIT, "clearing bcs[%d]", i);
-			gigaset_freebcs(cs->bcs + i);
-		}
-
+	case 0:	/* error in basic setup */
 		clear_events(cs);
 		gig_dbg(DEBUG_INIT, "freeing inbuf");
 		kfree(cs->inbuf);
@@ -534,16 +556,13 @@ void gigaset_at_init(struct at_state_t *at_state, struct bc_state *bcs,
 }
 
 
-static void gigaset_inbuf_init(struct inbuf_t *inbuf, struct bc_state *bcs,
-			       struct cardstate *cs, int inputstate)
+static void gigaset_inbuf_init(struct inbuf_t *inbuf, struct cardstate *cs)
 /* inbuf->read must be allocated before! */
 {
 	inbuf->head = 0;
 	inbuf->tail = 0;
 	inbuf->cs = cs;
-	inbuf->bcs = bcs; /*base driver: NULL*/
-	inbuf->rcvbuf = NULL;
-	inbuf->inputstate = inputstate;
+	inbuf->inputstate = INS_command;
 }
 
 /**
@@ -599,7 +618,7 @@ static struct bc_state *gigaset_initbcs(struct bc_state *bcs,
 {
 	int i;
 
-	bcs->tx_skb = NULL; //FIXME -> hw part
+	bcs->tx_skb = NULL;
 
 	skb_queue_head_init(&bcs->squeue);
 
@@ -618,13 +637,13 @@ static struct bc_state *gigaset_initbcs(struct bc_state *bcs,
 	bcs->fcs = PPP_INITFCS;
 	bcs->inputstate = 0;
 	if (cs->ignoreframes) {
-		bcs->inputstate |= INS_skip_frame;
 		bcs->skb = NULL;
-	} else if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
-		skb_reserve(bcs->skb, HW_HDR_LEN);
-	else {
-		pr_err("out of memory\n");
-		bcs->inputstate |= INS_skip_frame;
+	} else {
+		bcs->skb = dev_alloc_skb(SBUFSIZE + cs->hw_hdr_len);
+		if (bcs->skb != NULL)
+			skb_reserve(bcs->skb, cs->hw_hdr_len);
+		else
+			pr_err("out of memory\n");
 	}
 
 	bcs->channel = channel;
@@ -645,8 +664,8 @@ static struct bc_state *gigaset_initbcs(struct bc_state *bcs,
 	gig_dbg(DEBUG_INIT, "  failed");
 
 	gig_dbg(DEBUG_INIT, "  freeing bcs[%d]->skb", channel);
-	if (bcs->skb)
-		dev_kfree_skb(bcs->skb);
+	dev_kfree_skb(bcs->skb);
+	bcs->skb = NULL;
 
 	return NULL;
 }
@@ -673,12 +692,13 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 				 int onechannel, int ignoreframes,
 				 int cidmode, const char *modulename)
 {
-	struct cardstate *cs = NULL;
+	struct cardstate *cs;
 	unsigned long flags;
 	int i;
 
 	gig_dbg(DEBUG_INIT, "allocating cs");
-	if (!(cs = alloc_cs(drv))) {
+	cs = alloc_cs(drv);
+	if (!cs) {
 		pr_err("maximum number of devices exceeded\n");
 		return NULL;
 	}
@@ -707,7 +727,7 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	cs->ev_tail = 0;
 	cs->ev_head = 0;
 
-	tasklet_init(&cs->event_tasklet, &gigaset_handle_event,
+	tasklet_init(&cs->event_tasklet, gigaset_handle_event,
 		     (unsigned long) cs);
 	cs->commands_pending = 0;
 	cs->cur_at_seq = 0;
@@ -726,14 +746,6 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	cs->mode = M_UNKNOWN;
 	cs->mstate = MS_UNINITIALIZED;
 
-	for (i = 0; i < channels; ++i) {
-		gig_dbg(DEBUG_INIT, "setting up bcs[%d].read", i);
-		if (!gigaset_initbcs(cs->bcs + i, cs, i)) {
-			pr_err("could not allocate channel %d data\n", i);
-			goto error;
-		}
-	}
-
 	++cs->cs_init;
 
 	gig_dbg(DEBUG_INIT, "setting up at_state");
@@ -743,10 +755,7 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	cs->cbytes = 0;
 
 	gig_dbg(DEBUG_INIT, "setting up inbuf");
-	if (onechannel) {			//FIXME distinction necessary?
-		gigaset_inbuf_init(cs->inbuf, cs->bcs, cs, INS_command);
-	} else
-		gigaset_inbuf_init(cs->inbuf, NULL,    cs, INS_command);
+	gigaset_inbuf_init(cs->inbuf, cs);
 
 	cs->connected = 0;
 	cs->isdn_up = 0;
@@ -758,7 +767,7 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	cs->cmdbytes = 0;
 
 	gig_dbg(DEBUG_INIT, "setting up iif");
-	if (!gigaset_register_to_LL(cs, modulename)) {
+	if (!gigaset_isdn_register(cs, modulename)) {
 		pr_err("error registering ISDN device\n");
 		goto error;
 	}
@@ -777,6 +786,15 @@ struct cardstate *gigaset_initcs(struct gigaset_driver *drv, int channels,
 	/* set up device sysfs */
 	gigaset_init_dev_sysfs(cs);
 
+	/* set up channel data structures */
+	for (i = 0; i < channels; ++i) {
+		gig_dbg(DEBUG_INIT, "setting up bcs[%d]", i);
+		if (!gigaset_initbcs(cs->bcs + i, cs, i)) {
+			pr_err("could not allocate channel %d data\n", i);
+			goto error;
+		}
+	}
+
 	spin_lock_irqsave(&cs->lock, flags);
 	cs->running = 1;
 	spin_unlock_irqrestore(&cs->lock, flags);
@@ -824,9 +842,10 @@ void gigaset_bcs_reinit(struct bc_state *bcs)
 	bcs->chstate = 0;
 
 	bcs->ignore = cs->ignoreframes;
-	if (bcs->ignore)
-		bcs->inputstate |= INS_skip_frame;
-
+	if (bcs->ignore) {
+		dev_kfree_skb(bcs->skb);
+		bcs->skb = NULL;
+	}
 
 	cs->ops->reinitbcshw(bcs);
 }
@@ -847,8 +866,6 @@ static void cleanup_cs(struct cardstate *cs)
 	free_strings(&cs->at_state);
 	gigaset_at_init(&cs->at_state, NULL, cs, 0);
 
-	kfree(cs->inbuf->rcvbuf);
-	cs->inbuf->rcvbuf = NULL;
 	cs->inbuf->inputstate = INS_command;
 	cs->inbuf->head = 0;
 	cs->inbuf->tail = 0;
@@ -911,15 +928,13 @@ int gigaset_start(struct cardstate *cs)
 		cs->ops->baud_rate(cs, B115200);
 		cs->ops->set_line_ctrl(cs, CS8);
 		cs->control_state = TIOCM_DTR|TIOCM_RTS;
-	} else {
-		//FIXME use some saved values?
 	}
 
 	cs->waiting = 1;
 
 	if (!gigaset_add_event(cs, &cs->at_state, EV_START, NULL, 0, NULL)) {
 		cs->waiting = 0;
-		//FIXME what should we do?
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		goto error;
 	}
 
@@ -959,7 +974,7 @@ int gigaset_shutdown(struct cardstate *cs)
 	cs->waiting = 1;
 
 	if (!gigaset_add_event(cs, &cs->at_state, EV_SHUTDOWN, NULL, 0, NULL)) {
-		//FIXME what should we do?
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		goto exit;
 	}
 
@@ -990,7 +1005,7 @@ void gigaset_stop(struct cardstate *cs)
 	cs->waiting = 1;
 
 	if (!gigaset_add_event(cs, &cs->at_state, EV_STOP, NULL, 0, NULL)) {
-		//FIXME what should we do?
+		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		goto exit;
 	}
 
diff --git a/drivers/isdn/gigaset/dummyll.c b/drivers/isdn/gigaset/dummyll.c
new file mode 100644
index 000000000000..5b27c996af6d
--- /dev/null
+++ b/drivers/isdn/gigaset/dummyll.c
@@ -0,0 +1,68 @@
+/*
+ * Dummy LL interface for the Gigaset driver
+ *
+ * Copyright (c) 2009 by Tilman Schmidt <tilman@imap.cc>.
+ *
+ * =====================================================================
+ *	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.
+ * =====================================================================
+ */
+
+#include "gigaset.h"
+
+void gigaset_skb_sent(struct bc_state *bcs, struct sk_buff *skb)
+{
+}
+EXPORT_SYMBOL_GPL(gigaset_skb_sent);
+
+void gigaset_skb_rcvd(struct bc_state *bcs, struct sk_buff *skb)
+{
+}
+EXPORT_SYMBOL_GPL(gigaset_skb_rcvd);
+
+void gigaset_isdn_rcv_err(struct bc_state *bcs)
+{
+}
+EXPORT_SYMBOL_GPL(gigaset_isdn_rcv_err);
+
+int gigaset_isdn_icall(struct at_state_t *at_state)
+{
+	return ICALL_IGNORE;
+}
+
+void gigaset_isdn_connD(struct bc_state *bcs)
+{
+}
+
+void gigaset_isdn_hupD(struct bc_state *bcs)
+{
+}
+
+void gigaset_isdn_connB(struct bc_state *bcs)
+{
+}
+
+void gigaset_isdn_hupB(struct bc_state *bcs)
+{
+}
+
+void gigaset_isdn_start(struct cardstate *cs)
+{
+}
+
+void gigaset_isdn_stop(struct cardstate *cs)
+{
+}
+
+int gigaset_isdn_register(struct cardstate *cs, const char *isdnid)
+{
+	pr_info("no ISDN subsystem interface\n");
+	return 1;
+}
+
+void gigaset_isdn_unregister(struct cardstate *cs)
+{
+}
diff --git a/drivers/isdn/gigaset/ev-layer.c b/drivers/isdn/gigaset/ev-layer.c
index cc768caa38f5..ddeb0456d202 100644
--- a/drivers/isdn/gigaset/ev-layer.c
+++ b/drivers/isdn/gigaset/ev-layer.c
@@ -40,8 +40,8 @@
 
 /* Possible ASCII responses */
 #define RSP_OK		0
-//#define RSP_BUSY	1
-//#define RSP_CONNECT	2
+#define RSP_BUSY	1
+#define RSP_CONNECT	2
 #define RSP_ZGCI	3
 #define RSP_RING	4
 #define RSP_ZAOC	5
@@ -68,7 +68,6 @@
 #define RSP_ZHLC	(RSP_STR + STR_ZHLC)
 #define RSP_ERROR	-1	/* ERROR              */
 #define RSP_WRONG_CID	-2	/* unknown cid in cmd */
-//#define RSP_EMPTY	-3
 #define RSP_UNKNOWN	-4	/* unknown response   */
 #define RSP_FAIL	-5	/* internal error     */
 #define RSP_INVAL	-6	/* invalid response   */
@@ -76,9 +75,9 @@
 #define RSP_NONE	-19
 #define RSP_STRING	-20
 #define RSP_NULL	-21
-//#define RSP_RETRYFAIL	-22
-//#define RSP_RETRY	-23
-//#define RSP_SKIP	-24
+#define RSP_RETRYFAIL	-22
+#define RSP_RETRY	-23
+#define RSP_SKIP	-24
 #define RSP_INIT	-27
 #define RSP_ANY		-26
 #define RSP_LAST	-28
@@ -127,7 +126,6 @@
 #define ACT_NOTIFY_BC_UP	39
 #define ACT_DIAL		40
 #define ACT_ACCEPT		41
-#define ACT_PROTO_L2		42
 #define ACT_HUP			43
 #define ACT_IF_LOCK		44
 #define ACT_START		45
@@ -159,229 +157,229 @@
 #define SEQ_UMMODE	11
 
 
-// 100: init, 200: dle0, 250:dle1, 300: get cid (dial), 350: "hup" (no cid), 400: hup, 500: reset, 600: dial, 700: ring
+/* 100: init, 200: dle0, 250:dle1, 300: get cid (dial), 350: "hup" (no cid),
+ * 400: hup, 500: reset, 600: dial, 700: ring */
 struct reply_t gigaset_tab_nocid[] =
 {
-	/* resp_code, min_ConState, max_ConState, parameter, new_ConState, timeout, action, command */
-
-	/* initialize device, set cid mode if possible */
-	//{RSP_INIT,     -1, -1,100,                900, 0, {ACT_TEST}},
-	//{RSP_ERROR,   900,900, -1,                  0, 0, {ACT_FAILINIT}},
-	//{RSP_OK,      900,900, -1,                100, INIT_TIMEOUT,
-	//                                                  {ACT_TIMEOUT}},
-
-	{RSP_INIT,     -1, -1,SEQ_INIT,           100, INIT_TIMEOUT,
-							  {ACT_TIMEOUT}},                /* wait until device is ready */
-
-	{EV_TIMEOUT,  100,100, -1,                101, 3, {0},             "Z\r"},       /* device in transparent mode? try to initialize it. */
-	{RSP_OK,      101,103, -1,                120, 5, {ACT_GETSTRING}, "+GMR\r"},    /* get version */
-
-	{EV_TIMEOUT,  101,101, -1,                102, 5, {0},             "Z\r"},       /* timeout => try once again. */
-	{RSP_ERROR,   101,101, -1,                102, 5, {0},             "Z\r"},       /* error => try once again. */
-
-	{EV_TIMEOUT,  102,102, -1,                108, 5, {ACT_SETDLE1},   "^SDLE=0\r"}, /* timeout => try again in DLE mode. */
-	{RSP_OK,      108,108, -1,                104,-1},
-	{RSP_ZDLE,    104,104,  0,                103, 5, {0},             "Z\r"},
-	{EV_TIMEOUT,  104,104, -1,                  0, 0, {ACT_FAILINIT}},
-	{RSP_ERROR,   108,108, -1,                  0, 0, {ACT_FAILINIT}},
-
-	{EV_TIMEOUT,  108,108, -1,                105, 2, {ACT_SETDLE0,
-							   ACT_HUPMODEM,
-							   ACT_TIMEOUT}},                /* still timeout => connection in unimodem mode? */
-	{EV_TIMEOUT,  105,105, -1,                103, 5, {0},             "Z\r"},
-
-	{RSP_ERROR,   102,102, -1,                107, 5, {0},             "^GETPRE\r"}, /* ERROR on ATZ => maybe in config mode? */
-	{RSP_OK,      107,107, -1,                  0, 0, {ACT_CONFIGMODE}},
-	{RSP_ERROR,   107,107, -1,                  0, 0, {ACT_FAILINIT}},
-	{EV_TIMEOUT,  107,107, -1,                  0, 0, {ACT_FAILINIT}},
-
-	{RSP_ERROR,   103,103, -1,                  0, 0, {ACT_FAILINIT}},
-	{EV_TIMEOUT,  103,103, -1,                  0, 0, {ACT_FAILINIT}},
-
-	{RSP_STRING,  120,120, -1,                121,-1, {ACT_SETVER}},
-
-	{EV_TIMEOUT,  120,121, -1,                  0, 0, {ACT_FAILVER, ACT_INIT}},
-	{RSP_ERROR,   120,121, -1,                  0, 0, {ACT_FAILVER, ACT_INIT}},
-	{RSP_OK,      121,121, -1,                  0, 0, {ACT_GOTVER,  ACT_INIT}},
-
-	/* leave dle mode */
-	{RSP_INIT,      0,  0,SEQ_DLE0,           201, 5, {0},             "^SDLE=0\r"},
-	{RSP_OK,      201,201, -1,                202,-1},
-	{RSP_ZDLE,    202,202,  0,                  0, 0, {ACT_DLE0}},
-	{RSP_NODEV,   200,249, -1,                  0, 0, {ACT_FAKEDLE0}},
-	{RSP_ERROR,   200,249, -1,                  0, 0, {ACT_FAILDLE0}},
-	{EV_TIMEOUT,  200,249, -1,                  0, 0, {ACT_FAILDLE0}},
-
-	/* enter dle mode */
-	{RSP_INIT,      0,  0,SEQ_DLE1,           251, 5, {0},             "^SDLE=1\r"},
-	{RSP_OK,      251,251, -1,                252,-1},
-	{RSP_ZDLE,    252,252,  1,                  0, 0, {ACT_DLE1}},
-	{RSP_ERROR,   250,299, -1,                  0, 0, {ACT_FAILDLE1}},
-	{EV_TIMEOUT,  250,299, -1,                  0, 0, {ACT_FAILDLE1}},
-
-	/* incoming call */
-	{RSP_RING,     -1, -1, -1,                 -1,-1, {ACT_RING}},
-
-	/* get cid */
-	//{RSP_INIT,      0,  0,300,                901, 0, {ACT_TEST}},
-	//{RSP_ERROR,   901,901, -1,                  0, 0, {ACT_FAILCID}},
-	//{RSP_OK,      901,901, -1,                301, 5, {0},             "^SGCI?\r"},
-
-	{RSP_INIT,      0,  0,SEQ_CID,            301, 5, {0},             "^SGCI?\r"},
-	{RSP_OK,      301,301, -1,                302,-1},
-	{RSP_ZGCI,    302,302, -1,                  0, 0, {ACT_CID}},
-	{RSP_ERROR,   301,349, -1,                  0, 0, {ACT_FAILCID}},
-	{EV_TIMEOUT,  301,349, -1,                  0, 0, {ACT_FAILCID}},
-
-	/* enter cid mode */
-	{RSP_INIT,      0,  0,SEQ_CIDMODE,        150, 5, {0},             "^SGCI=1\r"},
-	{RSP_OK,      150,150, -1,                  0, 0, {ACT_CMODESET}},
-	{RSP_ERROR,   150,150, -1,                  0, 0, {ACT_FAILCMODE}},
-	{EV_TIMEOUT,  150,150, -1,                  0, 0, {ACT_FAILCMODE}},
-
-	/* leave cid mode */
-	//{RSP_INIT,      0,  0,SEQ_UMMODE,         160, 5, {0},             "^SGCI=0\r"},
-	{RSP_INIT,      0,  0,SEQ_UMMODE,         160, 5, {0},             "Z\r"},
-	{RSP_OK,      160,160, -1,                  0, 0, {ACT_UMODESET}},
-	{RSP_ERROR,   160,160, -1,                  0, 0, {ACT_FAILUMODE}},
-	{EV_TIMEOUT,  160,160, -1,                  0, 0, {ACT_FAILUMODE}},
-
-	/* abort getting cid */
-	{RSP_INIT,      0,  0,SEQ_NOCID,            0, 0, {ACT_ABORTCID}},
-
-	/* reset */
-	{RSP_INIT,      0,  0,SEQ_SHUTDOWN,       504, 5, {0},             "Z\r"},
-	{RSP_OK,      504,504, -1,                  0, 0, {ACT_SDOWN}},
-	{RSP_ERROR,   501,599, -1,                  0, 0, {ACT_FAILSDOWN}},
-	{EV_TIMEOUT,  501,599, -1,                  0, 0, {ACT_FAILSDOWN}},
-	{RSP_NODEV,   501,599, -1,                  0, 0, {ACT_FAKESDOWN}},
-
-	{EV_PROC_CIDMODE,-1, -1, -1,               -1,-1, {ACT_PROC_CIDMODE}}, //FIXME
-	{EV_IF_LOCK,   -1, -1, -1,                 -1,-1, {ACT_IF_LOCK}}, //FIXME
-	{EV_IF_VER,    -1, -1, -1,                 -1,-1, {ACT_IF_VER}}, //FIXME
-	{EV_START,     -1, -1, -1,                 -1,-1, {ACT_START}}, //FIXME
-	{EV_STOP,      -1, -1, -1,                 -1,-1, {ACT_STOP}}, //FIXME
-	{EV_SHUTDOWN,  -1, -1, -1,                 -1,-1, {ACT_SHUTDOWN}}, //FIXME
-
-	/* misc. */
-	{RSP_ERROR,    -1, -1, -1,                 -1, -1, {ACT_ERROR} },
-	{RSP_EMPTY,    -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZCFGT,    -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZCFG,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZLOG,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZMWI,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZABINFO,  -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZSMLSTCHG,-1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-
-	{RSP_ZCAU,     -1, -1, -1,                 -1,-1, {ACT_ZCAU}},
-	{RSP_NONE,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}},
-	{RSP_ANY,      -1, -1, -1,                 -1,-1, {ACT_WARN}},
-	{RSP_LAST}
+/* resp_code, min_ConState, max_ConState, parameter, new_ConState, timeout,
+ * action, command */
+
+/* initialize device, set cid mode if possible */
+{RSP_INIT,	 -1,  -1, SEQ_INIT,		100,  1, {ACT_TIMEOUT} },
+
+{EV_TIMEOUT,	100, 100, -1,			101,  3, {0},	"Z\r"},
+{RSP_OK,	101, 103, -1,			120,  5, {ACT_GETSTRING},
+								"+GMR\r"},
+
+{EV_TIMEOUT,	101, 101, -1,			102,  5, {0},	"Z\r"},
+{RSP_ERROR,	101, 101, -1,			102,  5, {0},	"Z\r"},
+
+{EV_TIMEOUT,	102, 102, -1,			108,  5, {ACT_SETDLE1},
+								"^SDLE=0\r"},
+{RSP_OK,	108, 108, -1,			104, -1},
+{RSP_ZDLE,	104, 104,  0,			103,  5, {0},	"Z\r"},
+{EV_TIMEOUT,	104, 104, -1,			  0,  0, {ACT_FAILINIT} },
+{RSP_ERROR,	108, 108, -1,			  0,  0, {ACT_FAILINIT} },
+
+{EV_TIMEOUT,	108, 108, -1,			105,  2, {ACT_SETDLE0,
+							  ACT_HUPMODEM,
+							  ACT_TIMEOUT} },
+{EV_TIMEOUT,	105, 105, -1,			103,  5, {0},	"Z\r"},
+
+{RSP_ERROR,	102, 102, -1,			107,  5, {0},	"^GETPRE\r"},
+{RSP_OK,	107, 107, -1,			  0,  0, {ACT_CONFIGMODE} },
+{RSP_ERROR,	107, 107, -1,			  0,  0, {ACT_FAILINIT} },
+{EV_TIMEOUT,	107, 107, -1,			  0,  0, {ACT_FAILINIT} },
+
+{RSP_ERROR,	103, 103, -1,			  0,  0, {ACT_FAILINIT} },
+{EV_TIMEOUT,	103, 103, -1,			  0,  0, {ACT_FAILINIT} },
+
+{RSP_STRING,	120, 120, -1,			121, -1, {ACT_SETVER} },
+
+{EV_TIMEOUT,	120, 121, -1,			  0,  0, {ACT_FAILVER,
+							  ACT_INIT} },
+{RSP_ERROR,	120, 121, -1,			  0,  0, {ACT_FAILVER,
+							  ACT_INIT} },
+{RSP_OK,	121, 121, -1,			  0,  0, {ACT_GOTVER,
+							  ACT_INIT} },
+
+/* leave dle mode */
+{RSP_INIT,	  0,   0, SEQ_DLE0,		201,  5, {0},	"^SDLE=0\r"},
+{RSP_OK,	201, 201, -1,			202, -1},
+{RSP_ZDLE,	202, 202,  0,			  0,  0, {ACT_DLE0} },
+{RSP_NODEV,	200, 249, -1,			  0,  0, {ACT_FAKEDLE0} },
+{RSP_ERROR,	200, 249, -1,			  0,  0, {ACT_FAILDLE0} },
+{EV_TIMEOUT,	200, 249, -1,			  0,  0, {ACT_FAILDLE0} },
+
+/* enter dle mode */
+{RSP_INIT,	  0,   0, SEQ_DLE1,		251,  5, {0},	"^SDLE=1\r"},
+{RSP_OK,	251, 251, -1,			252, -1},
+{RSP_ZDLE,	252, 252,  1,			  0,  0, {ACT_DLE1} },
+{RSP_ERROR,	250, 299, -1,			  0,  0, {ACT_FAILDLE1} },
+{EV_TIMEOUT,	250, 299, -1,			  0,  0, {ACT_FAILDLE1} },
+
+/* incoming call */
+{RSP_RING,	 -1,  -1, -1,			 -1, -1, {ACT_RING} },
+
+/* get cid */
+{RSP_INIT,	  0,   0, SEQ_CID,		301,  5, {0},	"^SGCI?\r"},
+{RSP_OK,	301, 301, -1,			302, -1},
+{RSP_ZGCI,	302, 302, -1,			  0,  0, {ACT_CID} },
+{RSP_ERROR,	301, 349, -1,			  0,  0, {ACT_FAILCID} },
+{EV_TIMEOUT,	301, 349, -1,			  0,  0, {ACT_FAILCID} },
+
+/* enter cid mode */
+{RSP_INIT,	  0,   0, SEQ_CIDMODE,		150,  5, {0},	"^SGCI=1\r"},
+{RSP_OK,	150, 150, -1,			  0,  0, {ACT_CMODESET} },
+{RSP_ERROR,	150, 150, -1,			  0,  0, {ACT_FAILCMODE} },
+{EV_TIMEOUT,	150, 150, -1,			  0,  0, {ACT_FAILCMODE} },
+
+/* leave cid mode */
+{RSP_INIT,	  0,   0, SEQ_UMMODE,		160,  5, {0},	"Z\r"},
+{RSP_OK,	160, 160, -1,			  0,  0, {ACT_UMODESET} },
+{RSP_ERROR,	160, 160, -1,			  0,  0, {ACT_FAILUMODE} },
+{EV_TIMEOUT,	160, 160, -1,			  0,  0, {ACT_FAILUMODE} },
+
+/* abort getting cid */
+{RSP_INIT,	  0,   0, SEQ_NOCID,		  0,  0, {ACT_ABORTCID} },
+
+/* reset */
+{RSP_INIT,	  0,   0, SEQ_SHUTDOWN,		504,  5, {0},	"Z\r"},
+{RSP_OK,	504, 504, -1,			  0,  0, {ACT_SDOWN} },
+{RSP_ERROR,	501, 599, -1,			  0,  0, {ACT_FAILSDOWN} },
+{EV_TIMEOUT,	501, 599, -1,			  0,  0, {ACT_FAILSDOWN} },
+{RSP_NODEV,	501, 599, -1,			  0,  0, {ACT_FAKESDOWN} },
+
+{EV_PROC_CIDMODE, -1, -1, -1,			 -1, -1, {ACT_PROC_CIDMODE} },
+{EV_IF_LOCK,	 -1,  -1, -1,			 -1, -1, {ACT_IF_LOCK} },
+{EV_IF_VER,	 -1,  -1, -1,			 -1, -1, {ACT_IF_VER} },
+{EV_START,	 -1,  -1, -1,			 -1, -1, {ACT_START} },
+{EV_STOP,	 -1,  -1, -1,			 -1, -1, {ACT_STOP} },
+{EV_SHUTDOWN,	 -1,  -1, -1,			 -1, -1, {ACT_SHUTDOWN} },
+
+/* misc. */
+{RSP_ERROR,	 -1,  -1, -1,			 -1, -1, {ACT_ERROR} },
+{RSP_ZCFGT,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZCFG,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZLOG,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZMWI,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZABINFO,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZSMLSTCHG,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+
+{RSP_ZCAU,	 -1,  -1, -1,			 -1, -1, {ACT_ZCAU} },
+{RSP_NONE,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ANY,	 -1,  -1, -1,			 -1, -1, {ACT_WARN} },
+{RSP_LAST}
 };
 
-// 600: start dialing, 650: dial in progress, 800: connection is up, 700: ring, 400: hup, 750: accepted icall
+/* 600: start dialing, 650: dial in progress, 800: connection is up, 700: ring,
+ * 400: hup, 750: accepted icall */
 struct reply_t gigaset_tab_cid[] =
 {
-	/* resp_code, min_ConState, max_ConState, parameter, new_ConState, timeout, action, command */
-
-	/* dial */
-	{EV_DIAL,      -1, -1, -1,                 -1,-1, {ACT_DIAL}}, //FIXME
-	{RSP_INIT,      0,  0,SEQ_DIAL,           601, 5, {ACT_CMD+AT_BC}},
-	{RSP_OK,      601,601, -1,                602, 5, {ACT_CMD+AT_HLC}},
-	{RSP_NULL,    602,602, -1,                603, 5, {ACT_CMD+AT_PROTO}},
-	{RSP_OK,      602,602, -1,                603, 5, {ACT_CMD+AT_PROTO}},
-	{RSP_OK,      603,603, -1,                604, 5, {ACT_CMD+AT_TYPE}},
-	{RSP_OK,      604,604, -1,                605, 5, {ACT_CMD+AT_MSN}},
-	{RSP_OK,      605,605, -1,                606, 5, {ACT_CMD+AT_ISO}},
-	{RSP_NULL,    605,605, -1,                606, 5, {ACT_CMD+AT_ISO}},
-	{RSP_OK,      606,606, -1,                607, 5, {0}, "+VLS=17\r"},
-	{RSP_OK,      607,607, -1,                608,-1},
-	{RSP_ZSAU,    608,608,ZSAU_PROCEEDING,    609, 5, {ACT_CMD+AT_DIAL}},
-	{RSP_OK,      609,609, -1,                650, 0, {ACT_DIALING}},
-
-	{RSP_ERROR,   601,609, -1,                  0, 0, {ACT_ABORTDIAL}},
-	{EV_TIMEOUT,  601,609, -1,                  0, 0, {ACT_ABORTDIAL}},
-
-	/* optional dialing responses */
-	{EV_BC_OPEN,  650,650, -1,                651,-1},
-	{RSP_ZVLS,    608,651, 17,                 -1,-1, {ACT_DEBUG}},
-	{RSP_ZCTP,    609,651, -1,                 -1,-1, {ACT_DEBUG}},
-	{RSP_ZCPN,    609,651, -1,                 -1,-1, {ACT_DEBUG}},
-	{RSP_ZSAU,    650,651,ZSAU_CALL_DELIVERED, -1,-1, {ACT_DEBUG}},
-
-	/* connect */
-	{RSP_ZSAU,    650,650,ZSAU_ACTIVE,        800,-1, {ACT_CONNECT}},
-	{RSP_ZSAU,    651,651,ZSAU_ACTIVE,        800,-1, {ACT_CONNECT,
-							   ACT_NOTIFY_BC_UP}},
-	{RSP_ZSAU,    750,750,ZSAU_ACTIVE,        800,-1, {ACT_CONNECT}},
-	{RSP_ZSAU,    751,751,ZSAU_ACTIVE,        800,-1, {ACT_CONNECT,
-							   ACT_NOTIFY_BC_UP}},
-	{EV_BC_OPEN,  800,800, -1,                800,-1, {ACT_NOTIFY_BC_UP}},
-
-	/* remote hangup */
-	{RSP_ZSAU,    650,651,ZSAU_DISCONNECT_IND,  0, 0, {ACT_REMOTEREJECT}},
-	{RSP_ZSAU,    750,751,ZSAU_DISCONNECT_IND,  0, 0, {ACT_REMOTEHUP}},
-	{RSP_ZSAU,    800,800,ZSAU_DISCONNECT_IND,  0, 0, {ACT_REMOTEHUP}},
-
-	/* hangup */
-	{EV_HUP,       -1, -1, -1,                 -1,-1, {ACT_HUP}}, //FIXME
-	{RSP_INIT,     -1, -1,SEQ_HUP,            401, 5, {0},             "+VLS=0\r"}, /* hang up */ //-1,-1?
-	{RSP_OK,      401,401, -1,                402, 5},
-	{RSP_ZVLS,    402,402,  0,                403, 5},
-	{RSP_ZSAU,    403, 403, ZSAU_DISCONNECT_REQ, -1, -1, {ACT_DEBUG} },
-	{RSP_ZSAU,    403, 403, ZSAU_NULL,            0,  0, {ACT_DISCONNECT} },
-	{RSP_NODEV,   401, 403, -1,                   0,  0, {ACT_FAKEHUP} },
-	{RSP_ERROR,   401,401, -1,                  0, 0, {ACT_ABORTHUP}},
-	{EV_TIMEOUT,  401,403, -1,                  0, 0, {ACT_ABORTHUP}},
-
-	{EV_BC_CLOSED,  0,  0, -1,                  0,-1, {ACT_NOTIFY_BC_DOWN}}, //FIXME new constate + timeout
-
-	/* ring */
-	{RSP_ZBC,     700,700, -1,                 -1,-1, {0}},
-	{RSP_ZHLC,    700,700, -1,                 -1,-1, {0}},
-	{RSP_NMBR,    700,700, -1,                 -1,-1, {0}},
-	{RSP_ZCPN,    700,700, -1,                 -1,-1, {0}},
-	{RSP_ZCTP,    700,700, -1,                 -1,-1, {0}},
-	{EV_TIMEOUT,  700,700, -1,               720,720, {ACT_ICALL}},
-	{EV_BC_CLOSED,720,720, -1,                  0,-1, {ACT_NOTIFY_BC_DOWN}},
-
-	/*accept icall*/
-	{EV_ACCEPT,    -1, -1, -1,                 -1,-1, {ACT_ACCEPT}}, //FIXME
-	{RSP_INIT,    720,720,SEQ_ACCEPT,         721, 5, {ACT_CMD+AT_PROTO}},
-	{RSP_OK,      721,721, -1,                722, 5, {ACT_CMD+AT_ISO}},
-	{RSP_OK,      722,722, -1,                723, 5, {0},             "+VLS=17\r"}, /* set "Endgeraetemodus" */
-	{RSP_OK,      723,723, -1,                724, 5, {0}},
-	{RSP_ZVLS,    724,724, 17,                750,50, {ACT_ACCEPTED}},
-	{RSP_ERROR,   721,729, -1,                  0, 0, {ACT_ABORTACCEPT}},
-	{EV_TIMEOUT,  721,729, -1,                  0, 0, {ACT_ABORTACCEPT}},
-	{RSP_ZSAU,    700,729,ZSAU_NULL,            0, 0, {ACT_ABORTACCEPT}},
-	{RSP_ZSAU,    700,729,ZSAU_ACTIVE,          0, 0, {ACT_ABORTACCEPT}},
-	{RSP_ZSAU,    700,729,ZSAU_DISCONNECT_IND,  0, 0, {ACT_ABORTACCEPT}},
-
-	{EV_BC_OPEN,  750,750, -1,                751,-1},
-	{EV_TIMEOUT,  750,751, -1,                  0, 0, {ACT_CONNTIMEOUT}},
-
-	/* B channel closed (general case) */
-	{EV_BC_CLOSED, -1, -1, -1,                 -1,-1, {ACT_NOTIFY_BC_DOWN}}, //FIXME
-
-	/* misc. */
-	{EV_PROTO_L2,  -1, -1, -1,                 -1,-1, {ACT_PROTO_L2}}, //FIXME
-
-	{RSP_ZCON,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZCCR,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZAOC,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-	{RSP_ZCSTR,    -1, -1, -1,                 -1,-1, {ACT_DEBUG}}, //FIXME
-
-	{RSP_ZCAU,     -1, -1, -1,                 -1,-1, {ACT_ZCAU}},
-	{RSP_NONE,     -1, -1, -1,                 -1,-1, {ACT_DEBUG}},
-	{RSP_ANY,      -1, -1, -1,                 -1,-1, {ACT_WARN}},
-	{RSP_LAST}
+/* resp_code, min_ConState, max_ConState, parameter, new_ConState, timeout,
+ * action, command */
+
+/* dial */
+{EV_DIAL,	 -1,  -1, -1,			 -1, -1, {ACT_DIAL} },
+{RSP_INIT,	  0,   0, SEQ_DIAL,		601,  5, {ACT_CMD+AT_BC} },
+{RSP_OK,	601, 601, -1,			602,  5, {ACT_CMD+AT_HLC} },
+{RSP_NULL,	602, 602, -1,			603,  5, {ACT_CMD+AT_PROTO} },
+{RSP_OK,	602, 602, -1,			603,  5, {ACT_CMD+AT_PROTO} },
+{RSP_OK,	603, 603, -1,			604,  5, {ACT_CMD+AT_TYPE} },
+{RSP_OK,	604, 604, -1,			605,  5, {ACT_CMD+AT_MSN} },
+{RSP_NULL,	605, 605, -1,			606,  5, {ACT_CMD+AT_CLIP} },
+{RSP_OK,	605, 605, -1,			606,  5, {ACT_CMD+AT_CLIP} },
+{RSP_NULL,	606, 606, -1,			607,  5, {ACT_CMD+AT_ISO} },
+{RSP_OK,	606, 606, -1,			607,  5, {ACT_CMD+AT_ISO} },
+{RSP_OK,	607, 607, -1,			608,  5, {0},	"+VLS=17\r"},
+{RSP_OK,	608, 608, -1,			609, -1},
+{RSP_ZSAU,	609, 609, ZSAU_PROCEEDING,	610,  5, {ACT_CMD+AT_DIAL} },
+{RSP_OK,	610, 610, -1,			650,  0, {ACT_DIALING} },
+
+{RSP_ERROR,	601, 610, -1,			  0,  0, {ACT_ABORTDIAL} },
+{EV_TIMEOUT,	601, 610, -1,			  0,  0, {ACT_ABORTDIAL} },
+
+/* optional dialing responses */
+{EV_BC_OPEN,	650, 650, -1,			651, -1},
+{RSP_ZVLS,	609, 651, 17,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZCTP,	610, 651, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZCPN,	610, 651, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZSAU,	650, 651, ZSAU_CALL_DELIVERED,	 -1, -1, {ACT_DEBUG} },
+
+/* connect */
+{RSP_ZSAU,	650, 650, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT} },
+{RSP_ZSAU,	651, 651, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT,
+							  ACT_NOTIFY_BC_UP} },
+{RSP_ZSAU,	750, 750, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT} },
+{RSP_ZSAU,	751, 751, ZSAU_ACTIVE,		800, -1, {ACT_CONNECT,
+							  ACT_NOTIFY_BC_UP} },
+{EV_BC_OPEN,	800, 800, -1,			800, -1, {ACT_NOTIFY_BC_UP} },
+
+/* remote hangup */
+{RSP_ZSAU,	650, 651, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_REMOTEREJECT} },
+{RSP_ZSAU,	750, 751, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_REMOTEHUP} },
+{RSP_ZSAU,	800, 800, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_REMOTEHUP} },
+
+/* hangup */
+{EV_HUP,	 -1,  -1, -1,			 -1, -1, {ACT_HUP} },
+{RSP_INIT,	 -1,  -1, SEQ_HUP,		401,  5, {0},	"+VLS=0\r"},
+{RSP_OK,	401, 401, -1,			402,  5},
+{RSP_ZVLS,	402, 402,  0,			403,  5},
+{RSP_ZSAU,	403, 403, ZSAU_DISCONNECT_REQ,	 -1, -1, {ACT_DEBUG} },
+{RSP_ZSAU,	403, 403, ZSAU_NULL,		  0,  0, {ACT_DISCONNECT} },
+{RSP_NODEV,	401, 403, -1,			  0,  0, {ACT_FAKEHUP} },
+{RSP_ERROR,	401, 401, -1,			  0,  0, {ACT_ABORTHUP} },
+{EV_TIMEOUT,	401, 403, -1,			  0,  0, {ACT_ABORTHUP} },
+
+{EV_BC_CLOSED,	  0,   0, -1,			  0, -1, {ACT_NOTIFY_BC_DOWN} },
+
+/* ring */
+{RSP_ZBC,	700, 700, -1,			 -1, -1, {0} },
+{RSP_ZHLC,	700, 700, -1,			 -1, -1, {0} },
+{RSP_NMBR,	700, 700, -1,			 -1, -1, {0} },
+{RSP_ZCPN,	700, 700, -1,			 -1, -1, {0} },
+{RSP_ZCTP,	700, 700, -1,			 -1, -1, {0} },
+{EV_TIMEOUT,	700, 700, -1,			720, 720, {ACT_ICALL} },
+{EV_BC_CLOSED,	720, 720, -1,			  0, -1, {ACT_NOTIFY_BC_DOWN} },
+
+/*accept icall*/
+{EV_ACCEPT,	 -1,  -1, -1,			 -1, -1, {ACT_ACCEPT} },
+{RSP_INIT,	720, 720, SEQ_ACCEPT,		721,  5, {ACT_CMD+AT_PROTO} },
+{RSP_OK,	721, 721, -1,			722,  5, {ACT_CMD+AT_ISO} },
+{RSP_OK,	722, 722, -1,			723,  5, {0},	"+VLS=17\r"},
+{RSP_OK,	723, 723, -1,			724,  5, {0} },
+{RSP_ZVLS,	724, 724, 17,			750, 50, {ACT_ACCEPTED} },
+{RSP_ERROR,	721, 729, -1,			  0,  0, {ACT_ABORTACCEPT} },
+{EV_TIMEOUT,	721, 729, -1,			  0,  0, {ACT_ABORTACCEPT} },
+{RSP_ZSAU,	700, 729, ZSAU_NULL,		  0,  0, {ACT_ABORTACCEPT} },
+{RSP_ZSAU,	700, 729, ZSAU_ACTIVE,		  0,  0, {ACT_ABORTACCEPT} },
+{RSP_ZSAU,	700, 729, ZSAU_DISCONNECT_IND,	  0,  0, {ACT_ABORTACCEPT} },
+
+{EV_BC_OPEN,	750, 750, -1,			751, -1},
+{EV_TIMEOUT,	750, 751, -1,			  0,  0, {ACT_CONNTIMEOUT} },
+
+/* B channel closed (general case) */
+{EV_BC_CLOSED,	 -1,  -1, -1,			 -1, -1, {ACT_NOTIFY_BC_DOWN} },
+
+/* misc. */
+{RSP_ZCON,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZCCR,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZAOC,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ZCSTR,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+
+{RSP_ZCAU,	 -1,  -1, -1,			 -1, -1, {ACT_ZCAU} },
+{RSP_NONE,	 -1,  -1, -1,			 -1, -1, {ACT_DEBUG} },
+{RSP_ANY,	 -1,  -1, -1,			 -1, -1, {ACT_WARN} },
+{RSP_LAST}
 };
 
 
-static const struct resp_type_t resp_type[] =
+static const struct resp_type_t {
+	unsigned char	*response;
+	int		resp_code;
+	int		type;
+} resp_type[] =
 {
-	/*{"",		RSP_EMPTY,	RT_NOTHING},*/
 	{"OK",		RSP_OK,		RT_NOTHING},
 	{"ERROR",	RSP_ERROR,	RT_NOTHING},
 	{"ZSAU",	RSP_ZSAU,	RT_ZSAU},
@@ -405,7 +403,21 @@ static const struct resp_type_t resp_type[] =
 	{"ZLOG",	RSP_ZLOG,	RT_NOTHING},
 	{"ZABINFO",	RSP_ZABINFO,	RT_NOTHING},
 	{"ZSMLSTCHG",	RSP_ZSMLSTCHG,	RT_NOTHING},
-	{NULL,0,0}
+	{NULL,		0,		0}
+};
+
+static const struct zsau_resp_t {
+	unsigned char	*str;
+	int		code;
+} zsau_resp[] =
+{
+	{"OUTGOING_CALL_PROCEEDING",	ZSAU_OUTGOING_CALL_PROCEEDING},
+	{"CALL_DELIVERED",		ZSAU_CALL_DELIVERED},
+	{"ACTIVE",			ZSAU_ACTIVE},
+	{"DISCONNECT_IND",		ZSAU_DISCONNECT_IND},
+	{"NULL",			ZSAU_NULL},
+	{"DISCONNECT_REQ",		ZSAU_DISCONNECT_REQ},
+	{NULL,				ZSAU_UNKNOWN}
 };
 
 /*
@@ -470,7 +482,6 @@ static int cid_of_response(char *s)
 	if (cid < 1 || cid > 65535)
 		return -1;	/* CID out of range */
 	return cid;
-	//FIXME is ;<digit>+ at end of non-CID response really impossible?
 }
 
 /**
@@ -487,6 +498,7 @@ void gigaset_handle_modem_response(struct cardstate *cs)
 	int params;
 	int i, j;
 	const struct resp_type_t *rt;
+	const struct zsau_resp_t *zr;
 	int curarg;
 	unsigned long flags;
 	unsigned next, tail, head;
@@ -613,24 +625,14 @@ void gigaset_handle_modem_response(struct cardstate *cs)
 				event->parameter = ZSAU_NONE;
 				break;
 			}
-			if (!strcmp(argv[curarg], "OUTGOING_CALL_PROCEEDING"))
-				event->parameter = ZSAU_OUTGOING_CALL_PROCEEDING;
-			else if (!strcmp(argv[curarg], "CALL_DELIVERED"))
-				event->parameter = ZSAU_CALL_DELIVERED;
-			else if (!strcmp(argv[curarg], "ACTIVE"))
-				event->parameter = ZSAU_ACTIVE;
-			else if (!strcmp(argv[curarg], "DISCONNECT_IND"))
-				event->parameter = ZSAU_DISCONNECT_IND;
-			else if (!strcmp(argv[curarg], "NULL"))
-				event->parameter = ZSAU_NULL;
-			else if (!strcmp(argv[curarg], "DISCONNECT_REQ"))
-				event->parameter = ZSAU_DISCONNECT_REQ;
-			else {
-				event->parameter = ZSAU_UNKNOWN;
+			for (zr = zsau_resp; zr->str; ++zr)
+				if (!strcmp(argv[curarg], zr->str))
+					break;
+			event->parameter = zr->code;
+			if (!zr->str)
 				dev_warn(cs->dev,
 					"%s: unknown parameter %s after ZSAU\n",
 					 __func__, argv[curarg]);
-			}
 			++curarg;
 			break;
 		case RT_STRING:
@@ -714,7 +716,7 @@ static void disconnect(struct at_state_t **at_state_p)
 		/* notify LL */
 		if (bcs->chstate & (CHS_D_UP | CHS_NOTIFY_LL)) {
 			bcs->chstate &= ~(CHS_D_UP | CHS_NOTIFY_LL);
-			gigaset_i4l_channel_cmd(bcs, ISDN_STAT_DHUP);
+			gigaset_isdn_hupD(bcs);
 		}
 	} else {
 		/* no B channel assigned: just deallocate */
@@ -872,12 +874,12 @@ static void bchannel_down(struct bc_state *bcs)
 {
 	if (bcs->chstate & CHS_B_UP) {
 		bcs->chstate &= ~CHS_B_UP;
-		gigaset_i4l_channel_cmd(bcs, ISDN_STAT_BHUP);
+		gigaset_isdn_hupB(bcs);
 	}
 
 	if (bcs->chstate & (CHS_D_UP | CHS_NOTIFY_LL)) {
 		bcs->chstate &= ~(CHS_D_UP | CHS_NOTIFY_LL);
-		gigaset_i4l_channel_cmd(bcs, ISDN_STAT_DHUP);
+		gigaset_isdn_hupD(bcs);
 	}
 
 	gigaset_free_channel(bcs);
@@ -894,15 +896,17 @@ static void bchannel_up(struct bc_state *bcs)
 	}
 
 	bcs->chstate |= CHS_B_UP;
-	gigaset_i4l_channel_cmd(bcs, ISDN_STAT_BCONN);
+	gigaset_isdn_connB(bcs);
 }
 
-static void start_dial(struct at_state_t *at_state, void *data, unsigned seq_index)
+static void start_dial(struct at_state_t *at_state, void *data,
+			unsigned seq_index)
 {
 	struct bc_state *bcs = at_state->bcs;
 	struct cardstate *cs = at_state->cs;
-	int retval;
+	char **commands = data;
 	unsigned long flags;
+	int i;
 
 	bcs->chstate |= CHS_NOTIFY_LL;
 
@@ -913,10 +917,10 @@ static void start_dial(struct at_state_t *at_state, void *data, unsigned seq_ind
 	}
 	spin_unlock_irqrestore(&cs->lock, flags);
 
-	retval = gigaset_isdn_setup_dial(at_state, data);
-	if (retval != 0)
-		goto error;
-
+	for (i = 0; i < AT_NUM; ++i) {
+		kfree(bcs->commands[i]);
+		bcs->commands[i] = commands[i];
+	}
 
 	at_state->pending_commands |= PC_CID;
 	gig_dbg(DEBUG_CMD, "Scheduling PC_CID");
@@ -924,6 +928,10 @@ static void start_dial(struct at_state_t *at_state, void *data, unsigned seq_ind
 	return;
 
 error:
+	for (i = 0; i < AT_NUM; ++i) {
+		kfree(commands[i]);
+		commands[i] = NULL;
+	}
 	at_state->pending_commands |= PC_NOCID;
 	gig_dbg(DEBUG_CMD, "Scheduling PC_NOCID");
 	cs->commands_pending = 1;
@@ -933,20 +941,31 @@ error:
 static void start_accept(struct at_state_t *at_state)
 {
 	struct cardstate *cs = at_state->cs;
-	int retval;
+	struct bc_state *bcs = at_state->bcs;
+	int i;
 
-	retval = gigaset_isdn_setup_accept(at_state);
+	for (i = 0; i < AT_NUM; ++i) {
+		kfree(bcs->commands[i]);
+		bcs->commands[i] = NULL;
+	}
 
-	if (retval == 0) {
-		at_state->pending_commands |= PC_ACCEPT;
-		gig_dbg(DEBUG_CMD, "Scheduling PC_ACCEPT");
-		cs->commands_pending = 1;
-	} else {
+	bcs->commands[AT_PROTO] = kmalloc(9, GFP_ATOMIC);
+	bcs->commands[AT_ISO] = kmalloc(9, GFP_ATOMIC);
+	if (!bcs->commands[AT_PROTO] || !bcs->commands[AT_ISO]) {
+		dev_err(at_state->cs->dev, "out of memory\n");
 		/* error reset */
 		at_state->pending_commands |= PC_HUP;
 		gig_dbg(DEBUG_CMD, "Scheduling PC_HUP");
 		cs->commands_pending = 1;
+		return;
 	}
+
+	snprintf(bcs->commands[AT_PROTO], 9, "^SBPR=%u\r", bcs->proto2);
+	snprintf(bcs->commands[AT_ISO], 9, "^SISO=%u\r", bcs->channel + 1);
+
+	at_state->pending_commands |= PC_ACCEPT;
+	gig_dbg(DEBUG_CMD, "Scheduling PC_ACCEPT");
+	cs->commands_pending = 1;
 }
 
 static void do_start(struct cardstate *cs)
@@ -957,9 +976,7 @@ static void do_start(struct cardstate *cs)
 		schedule_init(cs, MS_INIT);
 
 	cs->isdn_up = 1;
-	gigaset_i4l_cmd(cs, ISDN_STAT_RUN);
-					// FIXME: not in locked mode
-					// FIXME 2: only after init sequence
+	gigaset_isdn_start(cs);
 
 	cs->waiting = 0;
 	wake_up(&cs->waitqueue);
@@ -975,7 +992,7 @@ static void finish_shutdown(struct cardstate *cs)
 	/* Tell the LL that the device is not available .. */
 	if (cs->isdn_up) {
 		cs->isdn_up = 0;
-		gigaset_i4l_cmd(cs, ISDN_STAT_STOP);
+		gigaset_isdn_stop(cs);
 	}
 
 	/* The rest is done by cleanup_cs () in user mode. */
@@ -1113,7 +1130,6 @@ static int do_lock(struct cardstate *cs)
 
 		break;
 	case MS_LOCKED:
-		//retval = -EACCES;
 		break;
 	default:
 		return -EBUSY;
@@ -1276,7 +1292,7 @@ static void do_action(int action, struct cardstate *cs,
 			break;
 		}
 		bcs->chstate |= CHS_D_UP;
-		gigaset_i4l_channel_cmd(bcs, ISDN_STAT_DCONN);
+		gigaset_isdn_connD(bcs);
 		cs->ops->init_bchannel(bcs);
 		break;
 	case ACT_DLE1:
@@ -1284,7 +1300,7 @@ static void do_action(int action, struct cardstate *cs,
 		bcs = cs->bcs + cs->curchannel;
 
 		bcs->chstate |= CHS_D_UP;
-		gigaset_i4l_channel_cmd(bcs, ISDN_STAT_DCONN);
+		gigaset_isdn_connD(bcs);
 		cs->ops->init_bchannel(bcs);
 		break;
 	case ACT_FAKEHUP:
@@ -1369,7 +1385,7 @@ static void do_action(int action, struct cardstate *cs,
 		cs->cur_at_seq = SEQ_NONE;
 		break;
 
-	case ACT_ABORTACCEPT:	/* hangup/error/timeout during ICALL processing */
+	case ACT_ABORTACCEPT:	/* hangup/error/timeout during ICALL procssng */
 		disconnect(p_at_state);
 		break;
 
@@ -1443,17 +1459,6 @@ static void do_action(int action, struct cardstate *cs,
 			__func__, at_state->ConState);
 		cs->cur_at_seq = SEQ_NONE;
 		break;
-#ifdef CONFIG_GIGASET_DEBUG
-	case ACT_TEST:
-		{
-			static int count = 3; //2; //1;
-			*p_genresp = 1;
-			*p_resp_code = count ? RSP_ERROR : RSP_OK;
-			if (count > 0)
-				--count;
-		}
-		break;
-#endif
 	case ACT_DEBUG:
 		gig_dbg(DEBUG_ANY, "%s: resp_code %d in ConState %d",
 			__func__, ev->type, at_state->ConState);
@@ -1474,11 +1479,6 @@ static void do_action(int action, struct cardstate *cs,
 	case ACT_ACCEPT:
 		start_accept(at_state);
 		break;
-	case ACT_PROTO_L2:
-		gig_dbg(DEBUG_CMD, "set protocol to %u",
-			(unsigned) ev->parameter);
-		at_state->bcs->proto2 = ev->parameter;
-		break;
 	case ACT_HUP:
 		at_state->pending_commands |= PC_HUP;
 		cs->commands_pending = 1;
@@ -1493,7 +1493,7 @@ static void do_action(int action, struct cardstate *cs,
 		do_start(cs);
 		break;
 
-	/* events from the interface */ // FIXME without ACT_xxxx?
+	/* events from the interface */
 	case ACT_IF_LOCK:
 		cs->cmd_result = ev->parameter ? do_lock(cs) : do_unlock(cs);
 		cs->waiting = 0;
@@ -1512,7 +1512,7 @@ static void do_action(int action, struct cardstate *cs,
 		wake_up(&cs->waitqueue);
 		break;
 
-	/* events from the proc file system */ // FIXME without ACT_xxxx?
+	/* events from the proc file system */
 	case ACT_PROC_CIDMODE:
 		spin_lock_irqsave(&cs->lock, flags);
 		if (ev->parameter != cs->cidmode) {
@@ -1649,7 +1649,8 @@ static void process_event(struct cardstate *cs, struct event_t *ev)
 	for (curact = 0; curact < MAXACT; ++curact) {
 		/* The row tells us what we should do  ..
 		 */
-		do_action(rep->action[curact], cs, bcs, &at_state, &p_command, &genresp, &resp_code, ev);
+		do_action(rep->action[curact], cs, bcs, &at_state, &p_command,
+			  &genresp, &resp_code, ev);
 		if (!at_state)
 			break; /* may be freed after disconnect */
 	}
@@ -1661,13 +1662,14 @@ static void process_event(struct cardstate *cs, struct event_t *ev)
 
 		if (genresp) {
 			spin_lock_irqsave(&cs->lock, flags);
-			at_state->timer_expires = 0; //FIXME
-			at_state->timer_active = 0; //FIXME
+			at_state->timer_expires = 0;
+			at_state->timer_active = 0;
 			spin_unlock_irqrestore(&cs->lock, flags);
-			gigaset_add_event(cs, at_state, resp_code, NULL, 0, NULL);
+			gigaset_add_event(cs, at_state, resp_code,
+					  NULL, 0, NULL);
 		} else {
 			/* Send command to modem if not NULL... */
-			if (p_command/*rep->command*/) {
+			if (p_command) {
 				if (cs->connected)
 					send_command(cs, p_command,
 						     sendcid, cs->dle,
@@ -1754,7 +1756,8 @@ static void process_command_flags(struct cardstate *cs)
 		}
 	}
 
-	/* only switch back to unimodem mode, if no commands are pending and no channels are up */
+	/* only switch back to unimodem mode if no commands are pending and
+	 * no channels are up */
 	spin_lock_irqsave(&cs->lock, flags);
 	if (cs->at_state.pending_commands == PC_UMMODE
 	    && !cs->cidmode
@@ -1813,9 +1816,8 @@ static void process_command_flags(struct cardstate *cs)
 
 	if (cs->at_state.pending_commands & PC_INIT) {
 		cs->at_state.pending_commands &= ~PC_INIT;
-		cs->dle = 0; //FIXME
+		cs->dle = 0;
 		cs->inbuf->inputstate = INS_command;
-		//FIXME reset card state (or -> LOCK0)?
 		schedule_sequence(cs, &cs->at_state, SEQ_INIT);
 		return;
 	}
diff --git a/drivers/isdn/gigaset/gigaset.h b/drivers/isdn/gigaset/gigaset.h
index a2f6125739eb..e963a6c2e86d 100644
--- a/drivers/isdn/gigaset/gigaset.h
+++ b/drivers/isdn/gigaset/gigaset.h
@@ -23,7 +23,6 @@
 #include <linux/compiler.h>
 #include <linux/types.h>
 #include <linux/spinlock.h>
-#include <linux/isdnif.h>
 #include <linux/usb.h>
 #include <linux/skbuff.h>
 #include <linux/netdevice.h>
@@ -35,12 +34,11 @@
 #include <linux/list.h>
 #include <asm/atomic.h>
 
-#define GIG_VERSION {0,5,0,0}
-#define GIG_COMPAT  {0,4,0,0}
+#define GIG_VERSION {0, 5, 0, 0}
+#define GIG_COMPAT  {0, 4, 0, 0}
 
 #define MAX_REC_PARAMS 10	/* Max. number of params in response string */
 #define MAX_RESP_SIZE 512	/* Max. size of a response string */
-#define HW_HDR_LEN 2		/* Header size used to store ack info */
 
 #define MAX_EVENTS 64		/* size of event queue */
 
@@ -135,35 +133,32 @@ void gigaset_dbg_buffer(enum debuglevel level, const unsigned char *msg,
 #define OUT_VENDOR_REQ	(USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_ENDPOINT)
 #define IN_VENDOR_REQ	(USB_DIR_IN | USB_TYPE_VENDOR | USB_RECIP_ENDPOINT)
 
-/* int-in-events 3070 */
+/* interrupt pipe messages */
 #define HD_B1_FLOW_CONTROL		0x80
 #define HD_B2_FLOW_CONTROL		0x81
-#define HD_RECEIVEATDATA_ACK		(0x35)		// 3070
-						// att: HD_RECEIVE>>AT<<DATA_ACK
-#define HD_READY_SEND_ATDATA		(0x36)		// 3070
-#define HD_OPEN_ATCHANNEL_ACK		(0x37)		// 3070
-#define HD_CLOSE_ATCHANNEL_ACK		(0x38)		// 3070
-#define HD_DEVICE_INIT_OK		(0x11)		// ISurf USB + 3070
-#define HD_OPEN_B1CHANNEL_ACK		(0x51)		// ISurf USB + 3070
-#define HD_OPEN_B2CHANNEL_ACK		(0x52)		// ISurf USB + 3070
-#define HD_CLOSE_B1CHANNEL_ACK		(0x53)		// ISurf USB + 3070
-#define HD_CLOSE_B2CHANNEL_ACK		(0x54)		// ISurf USB + 3070
-// 	 Powermangment
-#define HD_SUSPEND_END			(0x61)		// ISurf USB
-//   Configuration
-#define HD_RESET_INTERRUPT_PIPE_ACK	(0xFF)		// ISurf USB + 3070
-
-/* control requests 3070 */
-#define	HD_OPEN_B1CHANNEL		(0x23)		// ISurf USB + 3070
-#define	HD_CLOSE_B1CHANNEL		(0x24)		// ISurf USB + 3070
-#define	HD_OPEN_B2CHANNEL		(0x25)		// ISurf USB + 3070
-#define	HD_CLOSE_B2CHANNEL		(0x26)		// ISurf USB + 3070
-#define HD_RESET_INTERRUPT_PIPE		(0x27)		// ISurf USB + 3070
-#define	HD_DEVICE_INIT_ACK		(0x34)		// ISurf USB + 3070
-#define	HD_WRITE_ATMESSAGE		(0x12)		// 3070
-#define	HD_READ_ATMESSAGE		(0x13)		// 3070
-#define	HD_OPEN_ATCHANNEL		(0x28)		// 3070
-#define	HD_CLOSE_ATCHANNEL		(0x29)		// 3070
+#define HD_RECEIVEATDATA_ACK		(0x35)		/* 3070 */
+#define HD_READY_SEND_ATDATA		(0x36)		/* 3070 */
+#define HD_OPEN_ATCHANNEL_ACK		(0x37)		/* 3070 */
+#define HD_CLOSE_ATCHANNEL_ACK		(0x38)		/* 3070 */
+#define HD_DEVICE_INIT_OK		(0x11)		/* ISurf USB + 3070 */
+#define HD_OPEN_B1CHANNEL_ACK		(0x51)		/* ISurf USB + 3070 */
+#define HD_OPEN_B2CHANNEL_ACK		(0x52)		/* ISurf USB + 3070 */
+#define HD_CLOSE_B1CHANNEL_ACK		(0x53)		/* ISurf USB + 3070 */
+#define HD_CLOSE_B2CHANNEL_ACK		(0x54)		/* ISurf USB + 3070 */
+#define HD_SUSPEND_END			(0x61)		/* ISurf USB */
+#define HD_RESET_INTERRUPT_PIPE_ACK	(0xFF)		/* ISurf USB + 3070 */
+
+/* control requests */
+#define	HD_OPEN_B1CHANNEL		(0x23)		/* ISurf USB + 3070 */
+#define	HD_CLOSE_B1CHANNEL		(0x24)		/* ISurf USB + 3070 */
+#define	HD_OPEN_B2CHANNEL		(0x25)		/* ISurf USB + 3070 */
+#define	HD_CLOSE_B2CHANNEL		(0x26)		/* ISurf USB + 3070 */
+#define HD_RESET_INTERRUPT_PIPE		(0x27)		/* ISurf USB + 3070 */
+#define	HD_DEVICE_INIT_ACK		(0x34)		/* ISurf USB + 3070 */
+#define	HD_WRITE_ATMESSAGE		(0x12)		/* 3070 */
+#define	HD_READ_ATMESSAGE		(0x13)		/* 3070 */
+#define	HD_OPEN_ATCHANNEL		(0x28)		/* 3070 */
+#define	HD_CLOSE_ATCHANNEL		(0x29)		/* 3070 */
 
 /* number of B channels supported by base driver */
 #define BAS_CHANNELS	2
@@ -193,7 +188,9 @@ void gigaset_dbg_buffer(enum debuglevel level, const unsigned char *msg,
 #define AT_PROTO	4
 #define AT_TYPE		5
 #define AT_HLC		6
-#define AT_NUM		7
+#define AT_CLIP		7
+/* total number */
+#define AT_NUM		8
 
 /* variables in struct at_state_t */
 #define VAR_ZSAU	0
@@ -216,7 +213,6 @@ void gigaset_dbg_buffer(enum debuglevel level, const unsigned char *msg,
 #define EV_START	-110
 #define EV_STOP		-111
 #define EV_IF_LOCK	-112
-#define EV_PROTO_L2	-113
 #define EV_ACCEPT	-114
 #define EV_DIAL		-115
 #define EV_HUP		-116
@@ -224,12 +220,11 @@ void gigaset_dbg_buffer(enum debuglevel level, const unsigned char *msg,
 #define EV_BC_CLOSED	-118
 
 /* input state */
-#define INS_command	0x0001
-#define INS_DLE_char	0x0002
+#define INS_command	0x0001	/* receiving messages (not payload data) */
+#define INS_DLE_char	0x0002	/* DLE flag received (in DLE mode) */
 #define INS_byte_stuff	0x0004
 #define INS_have_data	0x0008
-#define INS_skip_frame	0x0010
-#define INS_DLE_command	0x0020
+#define INS_DLE_command	0x0020	/* DLE message start (<DLE> X) received */
 #define INS_flag_hunt	0x0040
 
 /* channel state */
@@ -259,6 +254,11 @@ void gigaset_dbg_buffer(enum debuglevel level, const unsigned char *msg,
 #define SM_LOCKED	0
 #define SM_ISDN		1 /* default */
 
+/* layer 2 protocols (AT^SBPR=...) */
+#define L2_BITSYNC	0
+#define L2_HDLC		1
+#define L2_VOICE	2
+
 struct gigaset_ops;
 struct gigaset_driver;
 
@@ -286,8 +286,6 @@ extern struct reply_t gigaset_tab_cid[];
 extern struct reply_t gigaset_tab_nocid[];
 
 struct inbuf_t {
-	unsigned char		*rcvbuf;	/* usb-gigaset receive buffer */
-	struct bc_state		*bcs;
 	struct cardstate	*cs;
 	int			inputstate;
 	int			head, tail;
@@ -359,12 +357,6 @@ struct at_state_t {
 	struct bc_state		*bcs;
 };
 
-struct resp_type_t {
-	unsigned char	*response;
-	int		resp_code;	/* RSP_XXXX */
-	int		type;		/* RT_XXXX */
-};
-
 struct event_t {
 	int type;
 	void *ptr, *arg;
@@ -395,7 +387,7 @@ struct bc_state {
 
 	unsigned chstate;		/* bitmap (CHS_*) */
 	int ignore;
-	unsigned proto2;		/* Layer 2 protocol (ISDN_PROTO_L2_*) */
+	unsigned proto2;		/* layer 2 protocol (L2_*) */
 	char *commands[AT_NUM];		/* see AT_XXXX */
 
 #ifdef CONFIG_GIGASET_DEBUG
@@ -410,6 +402,8 @@ struct bc_state {
 		struct usb_bc_state *usb;	/* usb hardware driver (m105) */
 		struct bas_bc_state *bas;	/* usb hardware driver (base) */
 	} hw;
+
+	void *ap;			/* LL application structure */
 };
 
 struct cardstate {
@@ -456,12 +450,13 @@ struct cardstate {
 
 	unsigned running;		/* !=0 if events are handled */
 	unsigned connected;		/* !=0 if hardware is connected */
-	unsigned isdn_up;		/* !=0 after ISDN_STAT_RUN */
+	unsigned isdn_up;		/* !=0 after gigaset_isdn_start() */
 
 	unsigned cidmode;
 
 	int myid;			/* id for communication with LL */
-	isdn_if iif;
+	void *iif;			/* LL interface structure */
+	unsigned short hw_hdr_len;	/* headroom needed in data skbs */
 
 	struct reply_t *tabnocid;
 	struct reply_t *tabcid;
@@ -476,8 +471,8 @@ struct cardstate {
 
 	struct timer_list timer;
 	int retry_count;
-	int dle;			/* !=0 if modem commands/responses are
-					   dle encoded */
+	int dle;			/* !=0 if DLE mode is active
+					   (ZDLE=1 received -- M10x only) */
 	int cur_at_seq;			/* sequence of AT commands being
 					   processed */
 	int curchannel;			/* channel those commands are meant
@@ -616,7 +611,9 @@ struct gigaset_ops {
 	int (*baud_rate)(struct cardstate *cs, unsigned cflag);
 	int (*set_line_ctrl)(struct cardstate *cs, unsigned cflag);
 
-	/* Called from i4l.c to put an skb into the send-queue. */
+	/* Called from LL interface to put an skb into the send-queue.
+	 * After sending is completed, gigaset_skb_sent() must be called
+	 * with the skb's link layer header preserved. */
 	int (*send_skb)(struct bc_state *bcs, struct sk_buff *skb);
 
 	/* Called from ev-layer.c to process a block of data
@@ -625,7 +622,8 @@ struct gigaset_ops {
 
 };
 
-/* = Common structures and definitions ======================================= */
+/* = Common structures and definitions =======================================
+ */
 
 /* Parser states for DLE-Event:
  * <DLE-EVENT>: <DLE_FLAG> "X" <EVENT> <DLE_FLAG> "."
@@ -638,8 +636,7 @@ struct gigaset_ops {
  *  Functions implemented in asyncdata.c
  */
 
-/* Called from i4l.c to put an skb into the send-queue.
- * After sending gigaset_skb_sent() should be called. */
+/* Called from LL interface to put an skb into the send queue. */
 int gigaset_m10x_send_skb(struct bc_state *bcs, struct sk_buff *skb);
 
 /* Called from ev-layer.c to process a block of data
@@ -650,8 +647,7 @@ void gigaset_m10x_input(struct inbuf_t *inbuf);
  *  Functions implemented in isocdata.c
  */
 
-/* Called from i4l.c to put an skb into the send-queue.
- * After sending gigaset_skb_sent() should be called. */
+/* Called from LL interface to put an skb into the send queue. */
 int gigaset_isoc_send_skb(struct bc_state *bcs, struct sk_buff *skb);
 
 /* Called from ev-layer.c to process a block of data
@@ -674,36 +670,26 @@ void gigaset_isowbuf_init(struct isowbuf_t *iwb, unsigned char idle);
 int gigaset_isowbuf_getbytes(struct isowbuf_t *iwb, int size);
 
 /* ===========================================================================
- *  Functions implemented in i4l.c/gigaset.h
+ *  Functions implemented in LL interface
  */
 
-/* Called by gigaset_initcs() for setting up with the isdn4linux subsystem */
-int gigaset_register_to_LL(struct cardstate *cs, const char *isdnid);
+/* Called from common.c for setting up/shutting down with the ISDN subsystem */
+int gigaset_isdn_register(struct cardstate *cs, const char *isdnid);
+void gigaset_isdn_unregister(struct cardstate *cs);
 
-/* Called from xxx-gigaset.c to indicate completion of sending an skb */
+/* Called from hardware module to indicate completion of an skb */
 void gigaset_skb_sent(struct bc_state *bcs, struct sk_buff *skb);
+void gigaset_skb_rcvd(struct bc_state *bcs, struct sk_buff *skb);
+void gigaset_isdn_rcv_err(struct bc_state *bcs);
 
 /* Called from common.c/ev-layer.c to indicate events relevant to the LL */
+void gigaset_isdn_start(struct cardstate *cs);
+void gigaset_isdn_stop(struct cardstate *cs);
 int gigaset_isdn_icall(struct at_state_t *at_state);
-int gigaset_isdn_setup_accept(struct at_state_t *at_state);
-int gigaset_isdn_setup_dial(struct at_state_t *at_state, void *data);
-
-void gigaset_i4l_cmd(struct cardstate *cs, int cmd);
-void gigaset_i4l_channel_cmd(struct bc_state *bcs, int cmd);
-
-
-static inline void gigaset_isdn_rcv_err(struct bc_state *bcs)
-{
-	isdn_ctrl response;
-
-	/* error -> LL */
-	gig_dbg(DEBUG_CMD, "sending L1ERR");
-	response.driver = bcs->cs->myid;
-	response.command = ISDN_STAT_L1ERR;
-	response.arg = bcs->channel;
-	response.parm.errcode = ISDN_STAT_L1ERR_RECV;
-	bcs->cs->iif.statcallb(&response);
-}
+void gigaset_isdn_connD(struct bc_state *bcs);
+void gigaset_isdn_hupD(struct bc_state *bcs);
+void gigaset_isdn_connB(struct bc_state *bcs);
+void gigaset_isdn_hupB(struct bc_state *bcs);
 
 /* ===========================================================================
  *  Functions implemented in ev-layer.c
@@ -732,6 +718,7 @@ void gigaset_bcs_reinit(struct bc_state *bcs);
 void gigaset_at_init(struct at_state_t *at_state, struct bc_state *bcs,
 		     struct cardstate *cs, int cid);
 int gigaset_get_channel(struct bc_state *bcs);
+struct bc_state *gigaset_get_free_channel(struct cardstate *cs);
 void gigaset_free_channel(struct bc_state *bcs);
 int gigaset_get_channels(struct cardstate *cs);
 void gigaset_free_channels(struct cardstate *cs);
@@ -781,7 +768,7 @@ struct event_t *gigaset_add_event(struct cardstate *cs,
 				  void *ptr, int parameter, void *arg);
 
 /* Called on CONFIG1 command from frontend. */
-int gigaset_enterconfigmode(struct cardstate *cs); //0: success <0: errorcode
+int gigaset_enterconfigmode(struct cardstate *cs);
 
 /* cs->lock must not be locked */
 static inline void gigaset_schedule_event(struct cardstate *cs)
@@ -816,35 +803,6 @@ static inline void gigaset_bchannel_up(struct bc_state *bcs)
 /* handling routines for sk_buff */
 /* ============================= */
 
-/* pass received skb to LL
- * Warning: skb must not be accessed anymore!
- */
-static inline void gigaset_rcv_skb(struct sk_buff *skb,
-				   struct cardstate *cs,
-				   struct bc_state *bcs)
-{
-	cs->iif.rcvcallb_skb(cs->myid, bcs->channel, skb);
-	bcs->trans_down++;
-}
-
-/* handle reception of corrupted skb
- * Warning: skb must not be accessed anymore!
- */
-static inline void gigaset_rcv_error(struct sk_buff *procskb,
-				     struct cardstate *cs,
-				     struct bc_state *bcs)
-{
-	if (procskb)
-		dev_kfree_skb(procskb);
-
-	if (bcs->ignore)
-		--bcs->ignore;
-	else {
-		++bcs->corrupted;
-		gigaset_isdn_rcv_err(bcs);
-	}
-}
-
 /* append received bytes to inbuf */
 int gigaset_fill_inbuf(struct inbuf_t *inbuf, const unsigned char *src,
 		       unsigned numbytes);
diff --git a/drivers/isdn/gigaset/i4l.c b/drivers/isdn/gigaset/i4l.c
index 654489d836cd..c129ee47a8fb 100644
--- a/drivers/isdn/gigaset/i4l.c
+++ b/drivers/isdn/gigaset/i4l.c
@@ -14,6 +14,9 @@
  */
 
 #include "gigaset.h"
+#include <linux/isdnif.h>
+
+#define HW_HDR_LEN	2	/* Header size used to store ack info */
 
 /* == Handling of I4L IO =====================================================*/
 
@@ -36,12 +39,12 @@
 static int writebuf_from_LL(int driverID, int channel, int ack,
 			    struct sk_buff *skb)
 {
-	struct cardstate *cs;
+	struct cardstate *cs = gigaset_get_cs_by_id(driverID);
 	struct bc_state *bcs;
+	unsigned char *ack_header;
 	unsigned len;
-	unsigned skblen;
 
-	if (!(cs = gigaset_get_cs_by_id(driverID))) {
+	if (!cs) {
 		pr_err("%s: invalid driver ID (%d)\n", __func__, driverID);
 		return -ENODEV;
 	}
@@ -75,11 +78,23 @@ static int writebuf_from_LL(int driverID, int channel, int ack,
 		return -EINVAL;
 	}
 
-	skblen = ack ? len : 0;
-	skb->head[0] = skblen & 0xff;
-	skb->head[1] = skblen >> 8;
-	gig_dbg(DEBUG_MCMD, "skb: len=%u, skblen=%u: %02x %02x",
-		len, skblen, (unsigned) skb->head[0], (unsigned) skb->head[1]);
+	/* set up acknowledgement header */
+	if (skb_headroom(skb) < HW_HDR_LEN) {
+		/* should never happen */
+		dev_err(cs->dev, "%s: insufficient skb headroom\n", __func__);
+		return -ENOMEM;
+	}
+	skb_set_mac_header(skb, -HW_HDR_LEN);
+	skb->mac_len = HW_HDR_LEN;
+	ack_header = skb_mac_header(skb);
+	if (ack) {
+		ack_header[0] = len & 0xff;
+		ack_header[1] = len >> 8;
+	} else {
+		ack_header[0] = ack_header[1] = 0;
+	}
+	gig_dbg(DEBUG_MCMD, "skb: len=%u, ack=%d: %02x %02x",
+		len, ack, ack_header[0], ack_header[1]);
 
 	/* pass to device-specific module */
 	return cs->ops->send_skb(bcs, skb);
@@ -95,6 +110,8 @@ static int writebuf_from_LL(int driverID, int channel, int ack,
  */
 void gigaset_skb_sent(struct bc_state *bcs, struct sk_buff *skb)
 {
+	isdn_if *iif = bcs->cs->iif;
+	unsigned char *ack_header = skb_mac_header(skb);
 	unsigned len;
 	isdn_ctrl response;
 
@@ -104,8 +121,7 @@ void gigaset_skb_sent(struct bc_state *bcs, struct sk_buff *skb)
 		dev_warn(bcs->cs->dev, "%s: skb->len==%d\n",
 			 __func__, skb->len);
 
-	len = (unsigned char) skb->head[0] |
-	      (unsigned) (unsigned char) skb->head[1] << 8;
+	len = ack_header[0] + ((unsigned) ack_header[1] << 8);
 	if (len) {
 		gig_dbg(DEBUG_MCMD, "ACKing to LL (id: %d, ch: %d, sz: %u)",
 			bcs->cs->myid, bcs->channel, len);
@@ -114,71 +130,177 @@ void gigaset_skb_sent(struct bc_state *bcs, struct sk_buff *skb)
 		response.command = ISDN_STAT_BSENT;
 		response.arg = bcs->channel;
 		response.parm.length = len;
-		bcs->cs->iif.statcallb(&response);
+		iif->statcallb(&response);
 	}
 }
 EXPORT_SYMBOL_GPL(gigaset_skb_sent);
 
+/**
+ * gigaset_skb_rcvd() - pass received skb to LL
+ * @bcs:	B channel descriptor structure.
+ * @skb:	received data.
+ *
+ * Called by hardware module {bas,ser,usb}_gigaset when user data has
+ * been successfully received, for passing to the LL.
+ * Warning: skb must not be accessed anymore!
+ */
+void gigaset_skb_rcvd(struct bc_state *bcs, struct sk_buff *skb)
+{
+	isdn_if *iif = bcs->cs->iif;
+
+	iif->rcvcallb_skb(bcs->cs->myid, bcs->channel, skb);
+	bcs->trans_down++;
+}
+EXPORT_SYMBOL_GPL(gigaset_skb_rcvd);
+
+/**
+ * gigaset_isdn_rcv_err() - signal receive error
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by hardware module {bas,ser,usb}_gigaset when a receive error
+ * has occurred, for signalling to the LL.
+ */
+void gigaset_isdn_rcv_err(struct bc_state *bcs)
+{
+	isdn_if *iif = bcs->cs->iif;
+	isdn_ctrl response;
+
+	/* if currently ignoring packets, just count down */
+	if (bcs->ignore) {
+		bcs->ignore--;
+		return;
+	}
+
+	/* update statistics */
+	bcs->corrupted++;
+
+	/* error -> LL */
+	gig_dbg(DEBUG_CMD, "sending L1ERR");
+	response.driver = bcs->cs->myid;
+	response.command = ISDN_STAT_L1ERR;
+	response.arg = bcs->channel;
+	response.parm.errcode = ISDN_STAT_L1ERR_RECV;
+	iif->statcallb(&response);
+}
+EXPORT_SYMBOL_GPL(gigaset_isdn_rcv_err);
+
 /* This function will be called by LL to send commands
  * NOTE: LL ignores the returned value, for commands other than ISDN_CMD_IOCTL,
  * so don't put too much effort into it.
  */
 static int command_from_LL(isdn_ctrl *cntrl)
 {
-	struct cardstate *cs = gigaset_get_cs_by_id(cntrl->driver);
+	struct cardstate *cs;
 	struct bc_state *bcs;
 	int retval = 0;
-	struct setup_parm *sp;
+	char **commands;
+	int ch;
+	int i;
+	size_t l;
 
 	gigaset_debugdrivers();
 
-	if (!cs) {
+	gig_dbg(DEBUG_CMD, "driver: %d, command: %d, arg: 0x%lx",
+		cntrl->driver, cntrl->command, cntrl->arg);
+
+	cs = gigaset_get_cs_by_id(cntrl->driver);
+	if (cs == NULL) {
 		pr_err("%s: invalid driver ID (%d)\n", __func__, cntrl->driver);
 		return -ENODEV;
 	}
+	ch = cntrl->arg & 0xff;
 
 	switch (cntrl->command) {
 	case ISDN_CMD_IOCTL:
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_IOCTL (driver: %d, arg: %ld)",
-			cntrl->driver, cntrl->arg);
-
 		dev_warn(cs->dev, "ISDN_CMD_IOCTL not supported\n");
 		return -EINVAL;
 
 	case ISDN_CMD_DIAL:
 		gig_dbg(DEBUG_ANY,
-			"ISDN_CMD_DIAL (driver: %d, ch: %ld, "
-			"phone: %s, ownmsn: %s, si1: %d, si2: %d)",
-			cntrl->driver, cntrl->arg,
+			"ISDN_CMD_DIAL (phone: %s, msn: %s, si1: %d, si2: %d)",
 			cntrl->parm.setup.phone, cntrl->parm.setup.eazmsn,
 			cntrl->parm.setup.si1, cntrl->parm.setup.si2);
 
-		if (cntrl->arg >= cs->channels) {
+		if (ch >= cs->channels) {
 			dev_err(cs->dev,
-				"ISDN_CMD_DIAL: invalid channel (%d)\n",
-				(int) cntrl->arg);
+				"ISDN_CMD_DIAL: invalid channel (%d)\n", ch);
 			return -EINVAL;
 		}
-
-		bcs = cs->bcs + cntrl->arg;
-
+		bcs = cs->bcs + ch;
 		if (!gigaset_get_channel(bcs)) {
 			dev_err(cs->dev, "ISDN_CMD_DIAL: channel not free\n");
 			return -EBUSY;
 		}
 
-		sp = kmalloc(sizeof *sp, GFP_ATOMIC);
-		if (!sp) {
+		commands = kzalloc(AT_NUM*(sizeof *commands), GFP_ATOMIC);
+		if (!commands) {
 			gigaset_free_channel(bcs);
 			dev_err(cs->dev, "ISDN_CMD_DIAL: out of memory\n");
 			return -ENOMEM;
 		}
-		*sp = cntrl->parm.setup;
 
-		if (!gigaset_add_event(cs, &bcs->at_state, EV_DIAL, sp,
+		l = 3 + strlen(cntrl->parm.setup.phone);
+		commands[AT_DIAL] = kmalloc(l, GFP_ATOMIC);
+		if (!commands[AT_DIAL])
+			goto oom;
+		if (cntrl->parm.setup.phone[0] == '*' &&
+		    cntrl->parm.setup.phone[1] == '*') {
+			/* internal call: translate ** prefix to CTP value */
+			commands[AT_TYPE] = kstrdup("^SCTP=0\r", GFP_ATOMIC);
+			if (!commands[AT_TYPE])
+				goto oom;
+			snprintf(commands[AT_DIAL], l,
+				 "D%s\r", cntrl->parm.setup.phone+2);
+		} else {
+			commands[AT_TYPE] = kstrdup("^SCTP=1\r", GFP_ATOMIC);
+			if (!commands[AT_TYPE])
+				goto oom;
+			snprintf(commands[AT_DIAL], l,
+				 "D%s\r", cntrl->parm.setup.phone);
+		}
+
+		l = strlen(cntrl->parm.setup.eazmsn);
+		if (l) {
+			l += 8;
+			commands[AT_MSN] = kmalloc(l, GFP_ATOMIC);
+			if (!commands[AT_MSN])
+				goto oom;
+			snprintf(commands[AT_MSN], l, "^SMSN=%s\r",
+				 cntrl->parm.setup.eazmsn);
+		}
+
+		switch (cntrl->parm.setup.si1) {
+		case 1:		/* audio */
+			/* BC = 9090A3: 3.1 kHz audio, A-law */
+			commands[AT_BC] = kstrdup("^SBC=9090A3\r", GFP_ATOMIC);
+			if (!commands[AT_BC])
+				goto oom;
+			break;
+		case 7:		/* data */
+		default:	/* hope the app knows what it is doing */
+			/* BC = 8890: unrestricted digital information */
+			commands[AT_BC] = kstrdup("^SBC=8890\r", GFP_ATOMIC);
+			if (!commands[AT_BC])
+				goto oom;
+		}
+		/* ToDo: other si1 values, inspect si2, set HLC/LLC */
+
+		commands[AT_PROTO] = kmalloc(9, GFP_ATOMIC);
+		if (!commands[AT_PROTO])
+			goto oom;
+		snprintf(commands[AT_PROTO], 9, "^SBPR=%u\r", bcs->proto2);
+
+		commands[AT_ISO] = kmalloc(9, GFP_ATOMIC);
+		if (!commands[AT_ISO])
+			goto oom;
+		snprintf(commands[AT_ISO], 9, "^SISO=%u\r",
+			 (unsigned) bcs->channel + 1);
+
+		if (!gigaset_add_event(cs, &bcs->at_state, EV_DIAL, commands,
 				       bcs->at_state.seq_index, NULL)) {
-			//FIXME what should we do?
-			kfree(sp);
+			for (i = 0; i < AT_NUM; ++i)
+				kfree(commands[i]);
+			kfree(commands);
 			gigaset_free_channel(bcs);
 			return -ENOMEM;
 		}
@@ -186,115 +308,102 @@ static int command_from_LL(isdn_ctrl *cntrl)
 		gig_dbg(DEBUG_CMD, "scheduling DIAL");
 		gigaset_schedule_event(cs);
 		break;
-	case ISDN_CMD_ACCEPTD: //FIXME
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_ACCEPTD");
-
-		if (cntrl->arg >= cs->channels) {
+	case ISDN_CMD_ACCEPTD:
+		if (ch >= cs->channels) {
 			dev_err(cs->dev,
-				"ISDN_CMD_ACCEPTD: invalid channel (%d)\n",
-				(int) cntrl->arg);
+				"ISDN_CMD_ACCEPTD: invalid channel (%d)\n", ch);
 			return -EINVAL;
 		}
-
-		if (!gigaset_add_event(cs, &cs->bcs[cntrl->arg].at_state,
-				       EV_ACCEPT, NULL, 0, NULL)) {
-			//FIXME what should we do?
+		bcs = cs->bcs + ch;
+		if (!gigaset_add_event(cs, &bcs->at_state,
+				       EV_ACCEPT, NULL, 0, NULL))
 			return -ENOMEM;
-		}
 
 		gig_dbg(DEBUG_CMD, "scheduling ACCEPT");
 		gigaset_schedule_event(cs);
 
 		break;
 	case ISDN_CMD_ACCEPTB:
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_ACCEPTB");
 		break;
 	case ISDN_CMD_HANGUP:
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_HANGUP (ch: %d)",
-			(int) cntrl->arg);
-
-		if (cntrl->arg >= cs->channels) {
+		if (ch >= cs->channels) {
 			dev_err(cs->dev,
-				"ISDN_CMD_HANGUP: invalid channel (%d)\n",
-				(int) cntrl->arg);
+				"ISDN_CMD_HANGUP: invalid channel (%d)\n", ch);
 			return -EINVAL;
 		}
-
-		if (!gigaset_add_event(cs, &cs->bcs[cntrl->arg].at_state,
-				       EV_HUP, NULL, 0, NULL)) {
-			//FIXME what should we do?
+		bcs = cs->bcs + ch;
+		if (!gigaset_add_event(cs, &bcs->at_state,
+				       EV_HUP, NULL, 0, NULL))
 			return -ENOMEM;
-		}
 
 		gig_dbg(DEBUG_CMD, "scheduling HUP");
 		gigaset_schedule_event(cs);
 
 		break;
-	case ISDN_CMD_CLREAZ: /* Do not signal incoming signals */ //FIXME
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_CLREAZ");
+	case ISDN_CMD_CLREAZ: /* Do not signal incoming signals */
+		dev_info(cs->dev, "ignoring ISDN_CMD_CLREAZ\n");
 		break;
-	case ISDN_CMD_SETEAZ: /* Signal incoming calls for given MSN */ //FIXME
-		gig_dbg(DEBUG_ANY,
-			"ISDN_CMD_SETEAZ (id: %d, ch: %ld, number: %s)",
-			cntrl->driver, cntrl->arg, cntrl->parm.num);
+	case ISDN_CMD_SETEAZ: /* Signal incoming calls for given MSN */
+		dev_info(cs->dev, "ignoring ISDN_CMD_SETEAZ (%s)\n",
+			 cntrl->parm.num);
 		break;
 	case ISDN_CMD_SETL2: /* Set L2 to given protocol */
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_SETL2 (ch: %ld, proto: %lx)",
-			cntrl->arg & 0xff, (cntrl->arg >> 8));
-
-		if ((cntrl->arg & 0xff) >= cs->channels) {
+		if (ch >= cs->channels) {
 			dev_err(cs->dev,
-				"ISDN_CMD_SETL2: invalid channel (%d)\n",
-				(int) cntrl->arg & 0xff);
+				"ISDN_CMD_SETL2: invalid channel (%d)\n", ch);
 			return -EINVAL;
 		}
-
-		if (!gigaset_add_event(cs, &cs->bcs[cntrl->arg & 0xff].at_state,
-				       EV_PROTO_L2, NULL, cntrl->arg >> 8,
-				       NULL)) {
-			//FIXME what should we do?
-			return -ENOMEM;
+		bcs = cs->bcs + ch;
+		if (bcs->chstate & CHS_D_UP) {
+			dev_err(cs->dev,
+				"ISDN_CMD_SETL2: channel active (%d)\n", ch);
+			return -EINVAL;
+		}
+		switch (cntrl->arg >> 8) {
+		case ISDN_PROTO_L2_HDLC:
+			gig_dbg(DEBUG_CMD, "ISDN_CMD_SETL2: setting L2_HDLC");
+			bcs->proto2 = L2_HDLC;
+			break;
+		case ISDN_PROTO_L2_TRANS:
+			gig_dbg(DEBUG_CMD, "ISDN_CMD_SETL2: setting L2_VOICE");
+			bcs->proto2 = L2_VOICE;
+			break;
+		default:
+			dev_err(cs->dev,
+				"ISDN_CMD_SETL2: unsupported protocol (%lu)\n",
+				cntrl->arg >> 8);
+			return -EINVAL;
 		}
-
-		gig_dbg(DEBUG_CMD, "scheduling PROTO_L2");
-		gigaset_schedule_event(cs);
 		break;
 	case ISDN_CMD_SETL3: /* Set L3 to given protocol */
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_SETL3 (ch: %ld, proto: %lx)",
-			cntrl->arg & 0xff, (cntrl->arg >> 8));
-
-		if ((cntrl->arg & 0xff) >= cs->channels) {
+		if (ch >= cs->channels) {
 			dev_err(cs->dev,
-				"ISDN_CMD_SETL3: invalid channel (%d)\n",
-				(int) cntrl->arg & 0xff);
+				"ISDN_CMD_SETL3: invalid channel (%d)\n", ch);
 			return -EINVAL;
 		}
 
 		if (cntrl->arg >> 8 != ISDN_PROTO_L3_TRANS) {
 			dev_err(cs->dev,
-				"ISDN_CMD_SETL3: invalid protocol %lu\n",
+				"ISDN_CMD_SETL3: unsupported protocol (%lu)\n",
 				cntrl->arg >> 8);
 			return -EINVAL;
 		}
 
 		break;
 	case ISDN_CMD_PROCEED:
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_PROCEED"); //FIXME
+		gig_dbg(DEBUG_ANY, "ISDN_CMD_PROCEED");
 		break;
 	case ISDN_CMD_ALERT:
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_ALERT"); //FIXME
+		gig_dbg(DEBUG_ANY, "ISDN_CMD_ALERT");
 		if (cntrl->arg >= cs->channels) {
 			dev_err(cs->dev,
 				"ISDN_CMD_ALERT: invalid channel (%d)\n",
 				(int) cntrl->arg);
 			return -EINVAL;
 		}
-		//bcs = cs->bcs + cntrl->arg;
-		//bcs->proto2 = -1;
-		// FIXME
 		break;
 	case ISDN_CMD_REDIR:
-		gig_dbg(DEBUG_ANY, "ISDN_CMD_REDIR"); //FIXME
+		gig_dbg(DEBUG_ANY, "ISDN_CMD_REDIR");
 		break;
 	case ISDN_CMD_PROT_IO:
 		gig_dbg(DEBUG_ANY, "ISDN_CMD_PROT_IO");
@@ -324,149 +433,34 @@ static int command_from_LL(isdn_ctrl *cntrl)
 	}
 
 	return retval;
+
+oom:
+	dev_err(bcs->cs->dev, "out of memory\n");
+	for (i = 0; i < AT_NUM; ++i)
+		kfree(commands[i]);
+	return -ENOMEM;
 }
 
-void gigaset_i4l_cmd(struct cardstate *cs, int cmd)
+static void gigaset_i4l_cmd(struct cardstate *cs, int cmd)
 {
+	isdn_if *iif = cs->iif;
 	isdn_ctrl command;
 
 	command.driver = cs->myid;
 	command.command = cmd;
 	command.arg = 0;
-	cs->iif.statcallb(&command);
+	iif->statcallb(&command);
 }
 
-void gigaset_i4l_channel_cmd(struct bc_state *bcs, int cmd)
+static void gigaset_i4l_channel_cmd(struct bc_state *bcs, int cmd)
 {
+	isdn_if *iif = bcs->cs->iif;
 	isdn_ctrl command;
 
 	command.driver = bcs->cs->myid;
 	command.command = cmd;
 	command.arg = bcs->channel;
-	bcs->cs->iif.statcallb(&command);
-}
-
-int gigaset_isdn_setup_dial(struct at_state_t *at_state, void *data)
-{
-	struct bc_state *bcs = at_state->bcs;
-	unsigned proto;
-	const char *bc;
-	size_t length[AT_NUM];
-	size_t l;
-	int i;
-	struct setup_parm *sp = data;
-
-	switch (bcs->proto2) {
-	case ISDN_PROTO_L2_HDLC:
-		proto = 1; /* 0: Bitsynchron, 1: HDLC, 2: voice */
-		break;
-	case ISDN_PROTO_L2_TRANS:
-		proto = 2; /* 0: Bitsynchron, 1: HDLC, 2: voice */
-		break;
-	default:
-		dev_err(bcs->cs->dev, "%s: invalid L2 protocol: %u\n",
-			__func__, bcs->proto2);
-		return -EINVAL;
-	}
-
-	switch (sp->si1) {
-	case 1:		/* audio */
-		bc = "9090A3";	/* 3.1 kHz audio, A-law */
-		break;
-	case 7:		/* data */
-	default:	/* hope the app knows what it is doing */
-		bc = "8890";	/* unrestricted digital information */
-	}
-	//FIXME add missing si1 values from 1TR6, inspect si2, set HLC/LLC
-
-	length[AT_DIAL ] = 1 + strlen(sp->phone) + 1 + 1;
-	l = strlen(sp->eazmsn);
-	length[AT_MSN  ] = l ? 6 + l + 1 + 1 : 0;
-	length[AT_BC   ] = 5 + strlen(bc) + 1 + 1;
-	length[AT_PROTO] = 6 + 1 + 1 + 1; /* proto: 1 character */
-	length[AT_ISO  ] = 6 + 1 + 1 + 1; /* channel: 1 character */
-	length[AT_TYPE ] = 6 + 1 + 1 + 1; /* call type: 1 character */
-	length[AT_HLC  ] = 0;
-
-	for (i = 0; i < AT_NUM; ++i) {
-		kfree(bcs->commands[i]);
-		bcs->commands[i] = NULL;
-		if (length[i] &&
-		    !(bcs->commands[i] = kmalloc(length[i], GFP_ATOMIC))) {
-			dev_err(bcs->cs->dev, "out of memory\n");
-			return -ENOMEM;
-		}
-	}
-
-	/* type = 1: extern, 0: intern, 2: recall, 3: door, 4: centrex */
-	if (sp->phone[0] == '*' && sp->phone[1] == '*') {
-		/* internal call: translate ** prefix to CTP value */
-		snprintf(bcs->commands[AT_DIAL], length[AT_DIAL],
-			 "D%s\r", sp->phone+2);
-		strncpy(bcs->commands[AT_TYPE], "^SCTP=0\r", length[AT_TYPE]);
-	} else {
-		snprintf(bcs->commands[AT_DIAL], length[AT_DIAL],
-			 "D%s\r", sp->phone);
-		strncpy(bcs->commands[AT_TYPE], "^SCTP=1\r", length[AT_TYPE]);
-	}
-
-	if (bcs->commands[AT_MSN])
-		snprintf(bcs->commands[AT_MSN], length[AT_MSN],
-			 "^SMSN=%s\r", sp->eazmsn);
-	snprintf(bcs->commands[AT_BC   ], length[AT_BC   ],
-		 "^SBC=%s\r", bc);
-	snprintf(bcs->commands[AT_PROTO], length[AT_PROTO],
-		 "^SBPR=%u\r", proto);
-	snprintf(bcs->commands[AT_ISO  ], length[AT_ISO  ],
-		 "^SISO=%u\r", (unsigned)bcs->channel + 1);
-
-	return 0;
-}
-
-int gigaset_isdn_setup_accept(struct at_state_t *at_state)
-{
-	unsigned proto;
-	size_t length[AT_NUM];
-	int i;
-	struct bc_state *bcs = at_state->bcs;
-
-	switch (bcs->proto2) {
-	case ISDN_PROTO_L2_HDLC:
-		proto = 1; /* 0: Bitsynchron, 1: HDLC, 2: voice */
-		break;
-	case ISDN_PROTO_L2_TRANS:
-		proto = 2; /* 0: Bitsynchron, 1: HDLC, 2: voice */
-		break;
-	default:
-		dev_err(at_state->cs->dev, "%s: invalid protocol: %u\n",
-			__func__, bcs->proto2);
-		return -EINVAL;
-	}
-
-	length[AT_DIAL ] = 0;
-	length[AT_MSN  ] = 0;
-	length[AT_BC   ] = 0;
-	length[AT_PROTO] = 6 + 1 + 1 + 1; /* proto: 1 character */
-	length[AT_ISO  ] = 6 + 1 + 1 + 1; /* channel: 1 character */
-	length[AT_TYPE ] = 0;
-	length[AT_HLC  ] = 0;
-
-	for (i = 0; i < AT_NUM; ++i) {
-		kfree(bcs->commands[i]);
-		bcs->commands[i] = NULL;
-		if (length[i] &&
-		    !(bcs->commands[i] = kmalloc(length[i], GFP_ATOMIC))) {
-			dev_err(at_state->cs->dev, "out of memory\n");
-			return -ENOMEM;
-		}
-	}
-
-	snprintf(bcs->commands[AT_PROTO], length[AT_PROTO],
-		 "^SBPR=%u\r", proto);
-	snprintf(bcs->commands[AT_ISO  ], length[AT_ISO  ],
-		 "^SISO=%u\r", (unsigned) bcs->channel + 1);
-
-	return 0;
+	iif->statcallb(&command);
 }
 
 /**
@@ -482,13 +476,14 @@ int gigaset_isdn_icall(struct at_state_t *at_state)
 {
 	struct cardstate *cs = at_state->cs;
 	struct bc_state *bcs = at_state->bcs;
+	isdn_if *iif = cs->iif;
 	isdn_ctrl response;
 	int retval;
 
 	/* fill ICALL structure */
 	response.parm.setup.si1 = 0;	/* default: unknown */
 	response.parm.setup.si2 = 0;
-	response.parm.setup.screen = 0;	//FIXME how to set these?
+	response.parm.setup.screen = 0;
 	response.parm.setup.plan = 0;
 	if (!at_state->str_var[STR_ZBC]) {
 		/* no BC (internal call): assume speech, A-law */
@@ -509,29 +504,27 @@ int gigaset_isdn_icall(struct at_state_t *at_state)
 		return ICALL_IGNORE;
 	}
 	if (at_state->str_var[STR_NMBR]) {
-		strncpy(response.parm.setup.phone, at_state->str_var[STR_NMBR],
-			sizeof response.parm.setup.phone - 1);
-		response.parm.setup.phone[sizeof response.parm.setup.phone - 1] = 0;
+		strlcpy(response.parm.setup.phone, at_state->str_var[STR_NMBR],
+			sizeof response.parm.setup.phone);
 	} else
 		response.parm.setup.phone[0] = 0;
 	if (at_state->str_var[STR_ZCPN]) {
-		strncpy(response.parm.setup.eazmsn, at_state->str_var[STR_ZCPN],
-			sizeof response.parm.setup.eazmsn - 1);
-		response.parm.setup.eazmsn[sizeof response.parm.setup.eazmsn - 1] = 0;
+		strlcpy(response.parm.setup.eazmsn, at_state->str_var[STR_ZCPN],
+			sizeof response.parm.setup.eazmsn);
 	} else
 		response.parm.setup.eazmsn[0] = 0;
 
 	if (!bcs) {
 		dev_notice(cs->dev, "no channel for incoming call\n");
 		response.command = ISDN_STAT_ICALLW;
-		response.arg = 0; //FIXME
+		response.arg = 0;
 	} else {
 		gig_dbg(DEBUG_CMD, "Sending ICALL");
 		response.command = ISDN_STAT_ICALL;
-		response.arg = bcs->channel; //FIXME
+		response.arg = bcs->channel;
 	}
 	response.driver = cs->myid;
-	retval = cs->iif.statcallb(&response);
+	retval = iif->statcallb(&response);
 	gig_dbg(DEBUG_CMD, "Response: %d", retval);
 	switch (retval) {
 	case 0:	/* no takers */
@@ -560,16 +553,109 @@ int gigaset_isdn_icall(struct at_state_t *at_state)
 	}
 }
 
-/* Set Callback function pointer */
-int gigaset_register_to_LL(struct cardstate *cs, const char *isdnid)
+/**
+ * gigaset_isdn_connD() - signal D channel connect
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module to notify the LL that the D channel connection has
+ * been established.
+ */
+void gigaset_isdn_connD(struct bc_state *bcs)
 {
-	isdn_if *iif = &cs->iif;
+	gig_dbg(DEBUG_CMD, "sending DCONN");
+	gigaset_i4l_channel_cmd(bcs, ISDN_STAT_DCONN);
+}
 
-	gig_dbg(DEBUG_ANY, "Register driver capabilities to LL");
+/**
+ * gigaset_isdn_hupD() - signal D channel hangup
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module to notify the LL that the D channel connection has
+ * been shut down.
+ */
+void gigaset_isdn_hupD(struct bc_state *bcs)
+{
+	gig_dbg(DEBUG_CMD, "sending DHUP");
+	gigaset_i4l_channel_cmd(bcs, ISDN_STAT_DHUP);
+}
+
+/**
+ * gigaset_isdn_connB() - signal B channel connect
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module to notify the LL that the B channel connection has
+ * been established.
+ */
+void gigaset_isdn_connB(struct bc_state *bcs)
+{
+	gig_dbg(DEBUG_CMD, "sending BCONN");
+	gigaset_i4l_channel_cmd(bcs, ISDN_STAT_BCONN);
+}
+
+/**
+ * gigaset_isdn_hupB() - signal B channel hangup
+ * @bcs:	B channel descriptor structure.
+ *
+ * Called by main module to notify the LL that the B channel connection has
+ * been shut down.
+ */
+void gigaset_isdn_hupB(struct bc_state *bcs)
+{
+	gig_dbg(DEBUG_CMD, "sending BHUP");
+	gigaset_i4l_channel_cmd(bcs, ISDN_STAT_BHUP);
+}
+
+/**
+ * gigaset_isdn_start() - signal device availability
+ * @cs:		device descriptor structure.
+ *
+ * Called by main module to notify the LL that the device is available for
+ * use.
+ */
+void gigaset_isdn_start(struct cardstate *cs)
+{
+	gig_dbg(DEBUG_CMD, "sending RUN");
+	gigaset_i4l_cmd(cs, ISDN_STAT_RUN);
+}
+
+/**
+ * gigaset_isdn_stop() - signal device unavailability
+ * @cs:		device descriptor structure.
+ *
+ * Called by main module to notify the LL that the device is no longer
+ * available for use.
+ */
+void gigaset_isdn_stop(struct cardstate *cs)
+{
+	gig_dbg(DEBUG_CMD, "sending STOP");
+	gigaset_i4l_cmd(cs, ISDN_STAT_STOP);
+}
+
+/**
+ * gigaset_isdn_register() - register to LL
+ * @cs:		device descriptor structure.
+ * @isdnid:	device name.
+ *
+ * Called by main module to register the device with the LL.
+ *
+ * Return value: 1 for success, 0 for failure
+ */
+int gigaset_isdn_register(struct cardstate *cs, const char *isdnid)
+{
+	isdn_if *iif;
+
+	pr_info("ISDN4Linux interface\n");
+
+	iif = kmalloc(sizeof *iif, GFP_KERNEL);
+	if (!iif) {
+		pr_err("out of memory\n");
+		return 0;
+	}
 
 	if (snprintf(iif->id, sizeof iif->id, "%s_%u", isdnid, cs->minor_index)
 	    >= sizeof iif->id) {
 		pr_err("ID too long: %s\n", isdnid);
+		kfree(iif);
 		return 0;
 	}
 
@@ -593,9 +679,26 @@ int gigaset_register_to_LL(struct cardstate *cs, const char *isdnid)
 
 	if (!register_isdn(iif)) {
 		pr_err("register_isdn failed\n");
+		kfree(iif);
 		return 0;
 	}
 
+	cs->iif = iif;
 	cs->myid = iif->channels;		/* Set my device id */
+	cs->hw_hdr_len = HW_HDR_LEN;
 	return 1;
 }
+
+/**
+ * gigaset_isdn_unregister() - unregister from LL
+ * @cs:		device descriptor structure.
+ *
+ * Called by main module to unregister the device from the LL.
+ */
+void gigaset_isdn_unregister(struct cardstate *cs)
+{
+	gig_dbg(DEBUG_CMD, "sending UNLOAD");
+	gigaset_i4l_cmd(cs, ISDN_STAT_UNLOAD);
+	kfree(cs->iif);
+	cs->iif = NULL;
+}
diff --git a/drivers/isdn/gigaset/interface.c b/drivers/isdn/gigaset/interface.c
index 6a8e1384e7bd..d2260b0055fc 100644
--- a/drivers/isdn/gigaset/interface.c
+++ b/drivers/isdn/gigaset/interface.c
@@ -162,7 +162,7 @@ static int if_open(struct tty_struct *tty, struct file *filp)
 		return -ENODEV;
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return -ERESTARTSYS; // FIXME -EINTR?
+		return -ERESTARTSYS;
 	tty->driver_data = cs;
 
 	++cs->open_count;
@@ -171,7 +171,7 @@ static int if_open(struct tty_struct *tty, struct file *filp)
 		spin_lock_irqsave(&cs->lock, flags);
 		cs->tty = tty;
 		spin_unlock_irqrestore(&cs->lock, flags);
-		tty->low_latency = 1; //FIXME test
+		tty->low_latency = 1;
 	}
 
 	mutex_unlock(&cs->mutex);
@@ -228,7 +228,7 @@ static int if_ioctl(struct tty_struct *tty, struct file *file,
 	gig_dbg(DEBUG_IF, "%u: %s(0x%x)", cs->minor_index, __func__, cmd);
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return -ERESTARTSYS; // FIXME -EINTR?
+		return -ERESTARTSYS;
 
 	if (!cs->connected) {
 		gig_dbg(DEBUG_IF, "not connected");
@@ -299,9 +299,8 @@ static int if_tiocmget(struct tty_struct *tty, struct file *file)
 	gig_dbg(DEBUG_IF, "%u: %s()", cs->minor_index, __func__);
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return -ERESTARTSYS; // FIXME -EINTR?
+		return -ERESTARTSYS;
 
-	// FIXME read from device?
 	retval = cs->control_state & (TIOCM_RTS|TIOCM_DTR);
 
 	mutex_unlock(&cs->mutex);
@@ -326,7 +325,7 @@ static int if_tiocmset(struct tty_struct *tty, struct file *file,
 		cs->minor_index, __func__, set, clear);
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return -ERESTARTSYS; // FIXME -EINTR?
+		return -ERESTARTSYS;
 
 	if (!cs->connected) {
 		gig_dbg(DEBUG_IF, "not connected");
@@ -356,7 +355,7 @@ static int if_write(struct tty_struct *tty, const unsigned char *buf, int count)
 	gig_dbg(DEBUG_IF, "%u: %s()", cs->minor_index, __func__);
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return -ERESTARTSYS; // FIXME -EINTR?
+		return -ERESTARTSYS;
 
 	if (!cs->connected) {
 		gig_dbg(DEBUG_IF, "not connected");
@@ -390,7 +389,7 @@ static int if_write_room(struct tty_struct *tty)
 	gig_dbg(DEBUG_IF, "%u: %s()", cs->minor_index, __func__);
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return -ERESTARTSYS; // FIXME -EINTR?
+		return -ERESTARTSYS;
 
 	if (!cs->connected) {
 		gig_dbg(DEBUG_IF, "not connected");
@@ -455,9 +454,8 @@ static void if_throttle(struct tty_struct *tty)
 		gig_dbg(DEBUG_IF, "not connected");	/* nothing to do */
 	else if (!cs->open_count)
 		dev_warn(cs->dev, "%s: device not opened\n", __func__);
-	else {
-		//FIXME
-	}
+	else
+		gig_dbg(DEBUG_ANY, "%s: not implemented\n", __func__);
 
 	mutex_unlock(&cs->mutex);
 }
@@ -480,9 +478,8 @@ static void if_unthrottle(struct tty_struct *tty)
 		gig_dbg(DEBUG_IF, "not connected");	/* nothing to do */
 	else if (!cs->open_count)
 		dev_warn(cs->dev, "%s: device not opened\n", __func__);
-	else {
-		//FIXME
-	}
+	else
+		gig_dbg(DEBUG_ANY, "%s: not implemented\n", __func__);
 
 	mutex_unlock(&cs->mutex);
 }
@@ -515,10 +512,9 @@ static void if_set_termios(struct tty_struct *tty, struct ktermios *old)
 		goto out;
 	}
 
-	// stolen from mct_u232.c
 	iflag = tty->termios->c_iflag;
 	cflag = tty->termios->c_cflag;
-	old_cflag = old ? old->c_cflag : cflag; //FIXME?
+	old_cflag = old ? old->c_cflag : cflag;
 	gig_dbg(DEBUG_IF, "%u: iflag %x cflag %x old %x",
 		cs->minor_index, iflag, cflag, old_cflag);
 
@@ -588,7 +584,7 @@ void gigaset_if_init(struct cardstate *cs)
 	if (!drv->have_tty)
 		return;
 
-	tasklet_init(&cs->if_wake_tasklet, &if_wake, (unsigned long) cs);
+	tasklet_init(&cs->if_wake_tasklet, if_wake, (unsigned long) cs);
 
 	mutex_lock(&cs->mutex);
 	cs->tty_dev = tty_register_device(drv->tty, cs->minor_index, NULL);
@@ -632,7 +628,8 @@ void gigaset_if_receive(struct cardstate *cs,
 	struct tty_struct *tty;
 
 	spin_lock_irqsave(&cs->lock, flags);
-	if ((tty = cs->tty) == NULL)
+	tty = cs->tty;
+	if (tty == NULL)
 		gig_dbg(DEBUG_ANY, "receive on closed device");
 	else {
 		tty_buffer_request_room(tty, len);
@@ -659,9 +656,9 @@ void gigaset_if_initdriver(struct gigaset_driver *drv, const char *procname,
 
 	drv->have_tty = 0;
 
-	if ((drv->tty = alloc_tty_driver(minors)) == NULL)
+	drv->tty = tty = alloc_tty_driver(minors);
+	if (tty == NULL)
 		goto enomem;
-	tty = drv->tty;
 
 	tty->magic =		TTY_DRIVER_MAGIC,
 	tty->major =		GIG_MAJOR,
@@ -676,8 +673,8 @@ void gigaset_if_initdriver(struct gigaset_driver *drv, const char *procname,
 
 	tty->owner =		THIS_MODULE;
 
-	tty->init_termios          = tty_std_termios; //FIXME
-	tty->init_termios.c_cflag  = B9600 | CS8 | CREAD | HUPCL | CLOCAL; //FIXME
+	tty->init_termios          = tty_std_termios;
+	tty->init_termios.c_cflag  = B9600 | CS8 | CREAD | HUPCL | CLOCAL;
 	tty_set_operations(tty, &if_ops);
 
 	ret = tty_register_driver(tty);
diff --git a/drivers/isdn/gigaset/isocdata.c b/drivers/isdn/gigaset/isocdata.c
index 9f3ef7b4248c..85394a6ebae8 100644
--- a/drivers/isdn/gigaset/isocdata.c
+++ b/drivers/isdn/gigaset/isocdata.c
@@ -41,7 +41,8 @@ static inline int isowbuf_freebytes(struct isowbuf_t *iwb)
 
 	read = iwb->read;
 	write = iwb->write;
-	if ((freebytes = read - write) > 0) {
+	freebytes = read - write;
+	if (freebytes > 0) {
 		/* no wraparound: need padding space within regular area */
 		return freebytes - BAS_OUTBUFPAD;
 	} else if (read < BAS_OUTBUFPAD) {
@@ -53,29 +54,6 @@ static inline int isowbuf_freebytes(struct isowbuf_t *iwb)
 	}
 }
 
-/* compare two offsets within the buffer
- * The buffer is seen as circular, with the read position as start
- * returns -1/0/1 if position a </=/> position b without crossing 'read'
- */
-static inline int isowbuf_poscmp(struct isowbuf_t *iwb, int a, int b)
-{
-	int read;
-	if (a == b)
-		return 0;
-	read = iwb->read;
-	if (a < b) {
-		if (a < read && read <= b)
-			return +1;
-		else
-			return -1;
-	} else {
-		if (b < read && read <= a)
-			return -1;
-		else
-			return +1;
-	}
-}
-
 /* start writing
  * acquire the write semaphore
  * return true if acquired, false if busy
@@ -271,7 +249,7 @@ static inline void dump_bytes(enum debuglevel level, const char *tag,
  *        bit 14..13 = number of bits added by stuffing
  */
 static const u16 stufftab[5 * 256] = {
-// previous 1s = 0:
+/* previous 1s = 0: */
  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x000f,
  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x0017, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x201f,
  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x0027, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x002f,
@@ -289,7 +267,7 @@ static const u16 stufftab[5 * 256] = {
  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x0ce7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x0cef,
  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x10f7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x2ddf,
 
-// previous 1s = 1:
+/* previous 1s = 1: */
  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x0007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x200f,
  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x0017, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x202f,
  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x0027, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x204f,
@@ -307,7 +285,7 @@ static const u16 stufftab[5 * 256] = {
  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x0ce7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x2dcf,
  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x10f7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x31ef,
 
-// previous 1s = 2:
+/* previous 1s = 2: */
  0x0000, 0x0001, 0x0002, 0x0003, 0x0004, 0x0005, 0x0006, 0x2007, 0x0008, 0x0009, 0x000a, 0x000b, 0x000c, 0x000d, 0x000e, 0x2017,
  0x0010, 0x0011, 0x0012, 0x0013, 0x0014, 0x0015, 0x0016, 0x2027, 0x0018, 0x0019, 0x001a, 0x001b, 0x001c, 0x001d, 0x001e, 0x2037,
  0x0020, 0x0021, 0x0022, 0x0023, 0x0024, 0x0025, 0x0026, 0x2047, 0x0028, 0x0029, 0x002a, 0x002b, 0x002c, 0x002d, 0x002e, 0x2057,
@@ -325,7 +303,7 @@ static const u16 stufftab[5 * 256] = {
  0x0ce0, 0x0ce1, 0x0ce2, 0x0ce3, 0x0ce4, 0x0ce5, 0x0ce6, 0x2dc7, 0x0ce8, 0x0ce9, 0x0cea, 0x0ceb, 0x0cec, 0x0ced, 0x0cee, 0x2dd7,
  0x10f0, 0x10f1, 0x10f2, 0x10f3, 0x10f4, 0x10f5, 0x10f6, 0x31e7, 0x20f8, 0x20f9, 0x20fa, 0x20fb, 0x257c, 0x257d, 0x29be, 0x41f7,
 
-// previous 1s = 3:
+/* previous 1s = 3: */
  0x0000, 0x0001, 0x0002, 0x2003, 0x0004, 0x0005, 0x0006, 0x200b, 0x0008, 0x0009, 0x000a, 0x2013, 0x000c, 0x000d, 0x000e, 0x201b,
  0x0010, 0x0011, 0x0012, 0x2023, 0x0014, 0x0015, 0x0016, 0x202b, 0x0018, 0x0019, 0x001a, 0x2033, 0x001c, 0x001d, 0x001e, 0x203b,
  0x0020, 0x0021, 0x0022, 0x2043, 0x0024, 0x0025, 0x0026, 0x204b, 0x0028, 0x0029, 0x002a, 0x2053, 0x002c, 0x002d, 0x002e, 0x205b,
@@ -343,7 +321,7 @@ static const u16 stufftab[5 * 256] = {
  0x0ce0, 0x0ce1, 0x0ce2, 0x2dc3, 0x0ce4, 0x0ce5, 0x0ce6, 0x2dcb, 0x0ce8, 0x0ce9, 0x0cea, 0x2dd3, 0x0cec, 0x0ced, 0x0cee, 0x2ddb,
  0x10f0, 0x10f1, 0x10f2, 0x31e3, 0x10f4, 0x10f5, 0x10f6, 0x31eb, 0x20f8, 0x20f9, 0x20fa, 0x41f3, 0x257c, 0x257d, 0x29be, 0x46fb,
 
-// previous 1s = 4:
+/* previous 1s = 4: */
  0x0000, 0x2001, 0x0002, 0x2005, 0x0004, 0x2009, 0x0006, 0x200d, 0x0008, 0x2011, 0x000a, 0x2015, 0x000c, 0x2019, 0x000e, 0x201d,
  0x0010, 0x2021, 0x0012, 0x2025, 0x0014, 0x2029, 0x0016, 0x202d, 0x0018, 0x2031, 0x001a, 0x2035, 0x001c, 0x2039, 0x001e, 0x203d,
  0x0020, 0x2041, 0x0022, 0x2045, 0x0024, 0x2049, 0x0026, 0x204d, 0x0028, 0x2051, 0x002a, 0x2055, 0x002c, 0x2059, 0x002e, 0x205d,
@@ -367,7 +345,8 @@ static const u16 stufftab[5 * 256] = {
  * parameters:
  *	cin	input byte
  *	ones	number of trailing '1' bits in result before this step
- *	iwb	pointer to output buffer structure (write semaphore must be held)
+ *	iwb	pointer to output buffer structure
+ *		(write semaphore must be held)
  * return value:
  *	number of trailing '1' bits in result after this step
  */
@@ -408,7 +387,8 @@ static inline int hdlc_bitstuff_byte(struct isowbuf_t *iwb, unsigned char cin,
  * parameters:
  *	in	input buffer
  *	count	number of bytes in input buffer
- *	iwb	pointer to output buffer structure (write semaphore must be held)
+ *	iwb	pointer to output buffer structure
+ *		(write semaphore must be held)
  * return value:
  *	position of end of packet in output buffer on success,
  *	-EAGAIN if write semaphore busy or buffer full
@@ -440,7 +420,8 @@ static inline int hdlc_buildframe(struct isowbuf_t *iwb,
 		fcs = crc_ccitt_byte(fcs, c);
 	}
 
-	/* bitstuff and append FCS (complemented, least significant byte first) */
+	/* bitstuff and append FCS
+	 * (complemented, least significant byte first) */
 	fcs ^= 0xffff;
 	ones = hdlc_bitstuff_byte(iwb, fcs & 0x00ff, ones);
 	ones = hdlc_bitstuff_byte(iwb, (fcs >> 8) & 0x00ff, ones);
@@ -459,7 +440,8 @@ static inline int hdlc_buildframe(struct isowbuf_t *iwb,
  * parameters:
  *	in	input buffer
  *	count	number of bytes in input buffer
- *	iwb	pointer to output buffer structure (write semaphore must be held)
+ *	iwb	pointer to output buffer structure
+ *		(write semaphore must be held)
  * return value:
  *	position of end of packet in output buffer on success,
  *	-EAGAIN if write semaphore busy or buffer full
@@ -500,7 +482,7 @@ int gigaset_isoc_buildframe(struct bc_state *bcs, unsigned char *in, int len)
 	int result;
 
 	switch (bcs->proto2) {
-	case ISDN_PROTO_L2_HDLC:
+	case L2_HDLC:
 		result = hdlc_buildframe(bcs->hw.bas->isooutbuf, in, len);
 		gig_dbg(DEBUG_ISO, "%s: %d bytes HDLC -> %d",
 			__func__, len, result);
@@ -542,8 +524,9 @@ static inline void hdlc_flush(struct bc_state *bcs)
 	if (likely(bcs->skb != NULL))
 		skb_trim(bcs->skb, 0);
 	else if (!bcs->ignore) {
-		if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
-			skb_reserve(bcs->skb, HW_HDR_LEN);
+		bcs->skb = dev_alloc_skb(SBUFSIZE + bcs->cs->hw_hdr_len);
+		if (bcs->skb)
+			skb_reserve(bcs->skb, bcs->cs->hw_hdr_len);
 		else
 			dev_err(bcs->cs->dev, "could not allocate skb\n");
 	}
@@ -557,43 +540,46 @@ static inline void hdlc_flush(struct bc_state *bcs)
  */
 static inline void hdlc_done(struct bc_state *bcs)
 {
+	struct cardstate *cs = bcs->cs;
 	struct sk_buff *procskb;
+	unsigned int len;
 
 	if (unlikely(bcs->ignore)) {
 		bcs->ignore--;
 		hdlc_flush(bcs);
 		return;
 	}
-
-	if ((procskb = bcs->skb) == NULL) {
+	procskb = bcs->skb;
+	if (procskb == NULL) {
 		/* previous error */
 		gig_dbg(DEBUG_ISO, "%s: skb=NULL", __func__);
-		gigaset_rcv_error(NULL, bcs->cs, bcs);
+		gigaset_isdn_rcv_err(bcs);
 	} else if (procskb->len < 2) {
-		dev_notice(bcs->cs->dev, "received short frame (%d octets)\n",
+		dev_notice(cs->dev, "received short frame (%d octets)\n",
 			   procskb->len);
 		bcs->hw.bas->runts++;
-		gigaset_rcv_error(procskb, bcs->cs, bcs);
+		dev_kfree_skb_any(procskb);
+		gigaset_isdn_rcv_err(bcs);
 	} else if (bcs->fcs != PPP_GOODFCS) {
-		dev_notice(bcs->cs->dev, "frame check error (0x%04x)\n",
-			   bcs->fcs);
+		dev_notice(cs->dev, "frame check error (0x%04x)\n", bcs->fcs);
 		bcs->hw.bas->fcserrs++;
-		gigaset_rcv_error(procskb, bcs->cs, bcs);
+		dev_kfree_skb_any(procskb);
+		gigaset_isdn_rcv_err(bcs);
 	} else {
-		procskb->len -= 2;		/* subtract FCS */
-		procskb->tail -= 2;
-		gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)",
-			__func__, procskb->len);
+		len = procskb->len;
+		__skb_trim(procskb, len -= 2);	/* subtract FCS */
+		gig_dbg(DEBUG_ISO, "%s: good frame (%d octets)", __func__, len);
 		dump_bytes(DEBUG_STREAM_DUMP,
-			   "rcv data", procskb->data, procskb->len);
-		bcs->hw.bas->goodbytes += procskb->len;
-		gigaset_rcv_skb(procskb, bcs->cs, bcs);
+			   "rcv data", procskb->data, len);
+		bcs->hw.bas->goodbytes += len;
+		gigaset_skb_rcvd(bcs, procskb);
 	}
 
-	if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
-		skb_reserve(bcs->skb, HW_HDR_LEN);
+	bcs->skb = dev_alloc_skb(SBUFSIZE + cs->hw_hdr_len);
+	if (bcs->skb)
+		skb_reserve(bcs->skb, cs->hw_hdr_len);
 	else
-		dev_err(bcs->cs->dev, "could not allocate skb\n");
+		dev_err(cs->dev, "could not allocate skb\n");
 	bcs->fcs = PPP_INITFCS;
 }
 
@@ -610,12 +596,8 @@ static inline void hdlc_frag(struct bc_state *bcs, unsigned inbits)
 
 	dev_notice(bcs->cs->dev, "received partial byte (%d bits)\n", inbits);
 	bcs->hw.bas->alignerrs++;
-	gigaset_rcv_error(bcs->skb, bcs->cs, bcs);
-
-	if ((bcs->skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN)) != NULL)
-		skb_reserve(bcs->skb, HW_HDR_LEN);
-	else
-		dev_err(bcs->cs->dev, "could not allocate skb\n");
+	gigaset_isdn_rcv_err(bcs);
+	__skb_trim(bcs->skb, 0);
 	bcs->fcs = PPP_INITFCS;
 }
 
@@ -646,10 +628,10 @@ static const unsigned char bitcounts[256] = {
 };
 
 /* hdlc_unpack
- * perform HDLC frame processing (bit unstuffing, flag detection, FCS calculation)
- * on a sequence of received data bytes (8 bits each, LSB first)
- * pass on successfully received, complete frames as SKBs via gigaset_rcv_skb
- * notify of errors via gigaset_rcv_error
+ * perform HDLC frame processing (bit unstuffing, flag detection, FCS
+ * calculation) on a sequence of received data bytes (8 bits each, LSB first)
+ * pass on successfully received, complete frames as SKBs via gigaset_skb_rcvd
+ * notify of errors via gigaset_isdn_rcv_err
  * tally frames, errors etc. in BC structure counters
  * parameters:
  *	src	received data
@@ -665,9 +647,12 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 
 	/* load previous state:
 	 * inputstate = set of flag bits:
-	 * - INS_flag_hunt: no complete opening flag received since connection setup or last abort
-	 * - INS_have_data: at least one complete data byte received since last flag
-	 * seqlen = number of consecutive '1' bits in last 7 input stream bits (0..7)
+	 * - INS_flag_hunt: no complete opening flag received since connection
+	 *                  setup or last abort
+	 * - INS_have_data: at least one complete data byte received since last
+	 *                  flag
+	 * seqlen = number of consecutive '1' bits in last 7 input stream bits
+	 *          (0..7)
 	 * inbyte = accumulated partial data byte (if !INS_flag_hunt)
 	 * inbits = number of valid bits in inbyte, starting at LSB (0..6)
 	 */
@@ -701,9 +686,11 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 				inbyte = c >> (lead1 + 1);
 				inbits = 7 - lead1;
 				if (trail1 >= 8) {
-					/* interior stuffing: omitting the MSB handles most cases */
+					/* interior stuffing:
+					 * omitting the MSB handles most cases,
+					 * correct the incorrectly handled
+					 * cases individually */
 					inbits--;
-					/* correct the incorrectly handled cases individually */
 					switch (c) {
 					case 0xbe:
 						inbyte = 0x3f;
@@ -729,13 +716,14 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 			hdlc_flush(bcs);
 			inputstate |= INS_flag_hunt;
 		} else if (seqlen == 6) {
-			/* closing flag, including (6 - lead1) '1's and one '0' from inbits */
+			/* closing flag, including (6 - lead1) '1's
+			 * and one '0' from inbits */
 			if (inbits > 7 - lead1) {
 				hdlc_frag(bcs, inbits + lead1 - 7);
 				inputstate &= ~INS_have_data;
 			} else {
 				if (inbits < 7 - lead1)
-					ubc->stolen0s ++;
+					ubc->stolen0s++;
 				if (inputstate & INS_have_data) {
 					hdlc_done(bcs);
 					inputstate &= ~INS_have_data;
@@ -744,7 +732,7 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 
 			if (c == PPP_FLAG) {
 				/* complete flag, LSB overlaps preceding flag */
-				ubc->shared0s ++;
+				ubc->shared0s++;
 				inbits = 0;
 				inbyte = 0;
 			} else if (trail1 != 7) {
@@ -752,9 +740,11 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 				inbyte = c >> (lead1 + 1);
 				inbits = 7 - lead1;
 				if (trail1 >= 8) {
-					/* interior stuffing: omitting the MSB handles most cases */
+					/* interior stuffing:
+					 * omitting the MSB handles most cases,
+					 * correct the incorrectly handled
+					 * cases individually */
 					inbits--;
-					/* correct the incorrectly handled cases individually */
 					switch (c) {
 					case 0xbe:
 						inbyte = 0x3f;
@@ -762,7 +752,8 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 					}
 				}
 			} else {
-				/* abort sequence follows, skb already empty anyway */
+				/* abort sequence follows,
+				 * skb already empty anyway */
 				ubc->aborts++;
 				inputstate |= INS_flag_hunt;
 			}
@@ -787,14 +778,17 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 			} else {
 				/* stuffed data */
 				if (trail1 < 7) { /* => seqlen == 5 */
-					/* stuff bit at position lead1, no interior stuffing */
+					/* stuff bit at position lead1,
+					 * no interior stuffing */
 					unsigned char mask = (1 << lead1) - 1;
 					c = (c & mask) | ((c & ~mask) >> 1);
 					inbyte |= c << inbits;
 					inbits += 7;
 				} else if (seqlen < 5) { /* trail1 >= 8 */
-					/* interior stuffing: omitting the MSB handles most cases */
-					/* correct the incorrectly handled cases individually */
+					/* interior stuffing:
+					 * omitting the MSB handles most cases,
+					 * correct the incorrectly handled
+					 * cases individually */
 					switch (c) {
 					case 0xbe:
 						c = 0x7e;
@@ -804,8 +798,9 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 					inbits += 7;
 				} else { /* seqlen == 5 && trail1 >= 8 */
 
-					/* stuff bit at lead1 *and* interior stuffing */
-					switch (c) {	/* unstuff individually */
+					/* stuff bit at lead1 *and* interior
+					 * stuffing -- unstuff individually */
+					switch (c) {
 					case 0x7d:
 						c = 0x3f;
 						break;
@@ -841,7 +836,7 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 }
 
 /* trans_receive
- * pass on received USB frame transparently as SKB via gigaset_rcv_skb
+ * pass on received USB frame transparently as SKB via gigaset_skb_rcvd
  * invert bytes
  * tally frames, errors etc. in BC structure counters
  * parameters:
@@ -852,6 +847,7 @@ static inline void hdlc_unpack(unsigned char *src, unsigned count,
 static inline void trans_receive(unsigned char *src, unsigned count,
 				 struct bc_state *bcs)
 {
+	struct cardstate *cs = bcs->cs;
 	struct sk_buff *skb;
 	int dobytes;
 	unsigned char *dst;
@@ -861,13 +857,14 @@ static inline void trans_receive(unsigned char *src, unsigned count,
 		hdlc_flush(bcs);
 		return;
 	}
-	if (unlikely((skb = bcs->skb) == NULL)) {
-		bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
+	skb = bcs->skb;
+	if (unlikely(skb == NULL)) {
+		bcs->skb = skb = dev_alloc_skb(SBUFSIZE + cs->hw_hdr_len);
 		if (!skb) {
-			dev_err(bcs->cs->dev, "could not allocate skb\n");
+			dev_err(cs->dev, "could not allocate skb\n");
 			return;
 		}
-		skb_reserve(skb, HW_HDR_LEN);
+		skb_reserve(skb, cs->hw_hdr_len);
 	}
 	bcs->hw.bas->goodbytes += skb->len;
 	dobytes = TRANSBUFSIZE - skb->len;
@@ -881,23 +878,24 @@ static inline void trans_receive(unsigned char *src, unsigned count,
 		if (dobytes == 0) {
 			dump_bytes(DEBUG_STREAM_DUMP,
 				   "rcv data", skb->data, skb->len);
-			gigaset_rcv_skb(skb, bcs->cs, bcs);
-			bcs->skb = skb = dev_alloc_skb(SBUFSIZE + HW_HDR_LEN);
+			gigaset_skb_rcvd(bcs, skb);
+			bcs->skb = skb =
+				dev_alloc_skb(SBUFSIZE + cs->hw_hdr_len);
 			if (!skb) {
-				dev_err(bcs->cs->dev,
-					"could not allocate skb\n");
+				dev_err(cs->dev, "could not allocate skb\n");
 				return;
 			}
-			skb_reserve(bcs->skb, HW_HDR_LEN);
+			skb_reserve(skb, cs->hw_hdr_len);
 			dobytes = TRANSBUFSIZE;
 		}
 	}
 }
 
-void gigaset_isoc_receive(unsigned char *src, unsigned count, struct bc_state *bcs)
+void gigaset_isoc_receive(unsigned char *src, unsigned count,
+			  struct bc_state *bcs)
 {
 	switch (bcs->proto2) {
-	case ISDN_PROTO_L2_HDLC:
+	case L2_HDLC:
 		hdlc_unpack(src, count, bcs);
 		break;
 	default:		/* assume transparent */
@@ -981,8 +979,10 @@ void gigaset_isoc_input(struct inbuf_t *inbuf)
  * @bcs:	B channel descriptor structure.
  * @skb:	data to send.
  *
- * Called by i4l.c to queue an skb for sending, and start transmission if
+ * Called by LL to queue an skb for sending, and start transmission if
  * necessary.
+ * Once the payload data has been transmitted completely, gigaset_skb_sent()
+ * will be called with the skb's link layer header preserved.
  *
  * Return value:
  *	number of bytes accepted for sending (skb->len) if ok,
diff --git a/drivers/isdn/gigaset/proc.c b/drivers/isdn/gigaset/proc.c
index 9715aad9c3f0..758a00c1d2e2 100644
--- a/drivers/isdn/gigaset/proc.c
+++ b/drivers/isdn/gigaset/proc.c
@@ -39,7 +39,7 @@ static ssize_t set_cidmode(struct device *dev, struct device_attribute *attr,
 			return -EINVAL;
 
 	if (mutex_lock_interruptible(&cs->mutex))
-		return -ERESTARTSYS; // FIXME -EINTR?
+		return -ERESTARTSYS;
 
 	cs->waiting = 1;
 	if (!gigaset_add_event(cs, &cs->at_state, EV_PROC_CIDMODE,
diff --git a/drivers/isdn/gigaset/ser-gigaset.c b/drivers/isdn/gigaset/ser-gigaset.c
index 3071a52467ed..168d585d64d8 100644
--- a/drivers/isdn/gigaset/ser-gigaset.c
+++ b/drivers/isdn/gigaset/ser-gigaset.c
@@ -164,9 +164,15 @@ static void gigaset_modem_fill(unsigned long data)
 {
 	struct cardstate *cs = (struct cardstate *) data;
 	struct bc_state *bcs;
+	struct sk_buff *nextskb;
 	int sent = 0;
 
-	if (!cs || !(bcs = cs->bcs)) {
+	if (!cs) {
+		gig_dbg(DEBUG_OUTPUT, "%s: no cardstate", __func__);
+		return;
+	}
+	bcs = cs->bcs;
+	if (!bcs) {
 		gig_dbg(DEBUG_OUTPUT, "%s: no cardstate", __func__);
 		return;
 	}
@@ -179,9 +185,11 @@ static void gigaset_modem_fill(unsigned long data)
 			return;
 
 		/* no command to send; get skb */
-		if (!(bcs->tx_skb = skb_dequeue(&bcs->squeue)))
+		nextskb = skb_dequeue(&bcs->squeue);
+		if (!nextskb)
 			/* no skb either, nothing to do */
 			return;
+		bcs->tx_skb = nextskb;
 
 		gig_dbg(DEBUG_INTR, "Dequeued skb (Adr: %lx)",
 			(unsigned long) bcs->tx_skb);
@@ -236,19 +244,20 @@ static void flush_send_queue(struct cardstate *cs)
  *	number of bytes queued, or error code < 0
  */
 static int gigaset_write_cmd(struct cardstate *cs, const unsigned char *buf,
-                             int len, struct tasklet_struct *wake_tasklet)
+			     int len, struct tasklet_struct *wake_tasklet)
 {
 	struct cmdbuf_t *cb;
 	unsigned long flags;
 
 	gigaset_dbg_buffer(cs->mstate != MS_LOCKED ?
-	                     DEBUG_TRANSCMD : DEBUG_LOCKCMD,
-	                   "CMD Transmit", len, buf);
+				DEBUG_TRANSCMD : DEBUG_LOCKCMD,
+			   "CMD Transmit", len, buf);
 
 	if (len <= 0)
 		return 0;
 
-	if (!(cb = kmalloc(sizeof(struct cmdbuf_t) + len, GFP_ATOMIC))) {
+	cb = kmalloc(sizeof(struct cmdbuf_t) + len, GFP_ATOMIC);
+	if (!cb) {
 		dev_err(cs->dev, "%s: out of memory!\n", __func__);
 		return -ENOMEM;
 	}
@@ -392,7 +401,6 @@ static void gigaset_device_release(struct device *dev)
 	struct platform_device *pdev = to_platform_device(dev);
 
 	/* adapted from platform_device_release() in drivers/base/platform.c */
-	//FIXME is this actually necessary?
 	kfree(dev->platform_data);
 	kfree(pdev->resource);
 }
@@ -404,16 +412,20 @@ static void gigaset_device_release(struct device *dev)
 static int gigaset_initcshw(struct cardstate *cs)
 {
 	int rc;
+	struct ser_cardstate *scs;
 
-	if (!(cs->hw.ser = kzalloc(sizeof(struct ser_cardstate), GFP_KERNEL))) {
+	scs = kzalloc(sizeof(struct ser_cardstate), GFP_KERNEL);
+	if (!scs) {
 		pr_err("out of memory\n");
 		return 0;
 	}
+	cs->hw.ser = scs;
 
 	cs->hw.ser->dev.name = GIGASET_MODULENAME;
 	cs->hw.ser->dev.id = cs->minor_index;
 	cs->hw.ser->dev.dev.release = gigaset_device_release;
-	if ((rc = platform_device_register(&cs->hw.ser->dev)) != 0) {
+	rc = platform_device_register(&cs->hw.ser->dev);
+	if (rc != 0) {
 		pr_err("error %d registering platform device\n", rc);
 		kfree(cs->hw.ser);
 		cs->hw.ser = NULL;
@@ -422,7 +434,7 @@ static int gigaset_initcshw(struct cardstate *cs)
 	dev_set_drvdata(&cs->hw.ser->dev.dev, cs);
 
 	tasklet_init(&cs->write_tasklet,
-	             &gigaset_modem_fill, (unsigned long) cs);
+		     gigaset_modem_fill, (unsigned long) cs);
 	return 1;
 }
 
@@ -434,7 +446,8 @@ static int gigaset_initcshw(struct cardstate *cs)
  * Called by "gigaset_start" and "gigaset_enterconfigmode" in common.c
  * and by "if_lock" and "if_termios" in interface.c
  */
-static int gigaset_set_modem_ctrl(struct cardstate *cs, unsigned old_state, unsigned new_state)
+static int gigaset_set_modem_ctrl(struct cardstate *cs, unsigned old_state,
+				  unsigned new_state)
 {
 	struct tty_struct *tty = cs->hw.ser->tty;
 	unsigned int set, clear;
@@ -520,8 +533,8 @@ gigaset_tty_open(struct tty_struct *tty)
 	}
 
 	/* allocate memory for our device state and intialize it */
-	if (!(cs = gigaset_initcs(driver, 1, 1, 0, cidmode,
-				  GIGASET_MODULENAME)))
+	cs = gigaset_initcs(driver, 1, 1, 0, cidmode, GIGASET_MODULENAME);
+	if (!cs)
 		goto error;
 
 	cs->dev = &cs->hw.ser->dev.dev;
@@ -690,7 +703,8 @@ gigaset_tty_receive(struct tty_struct *tty, const unsigned char *buf,
 
 	if (!cs)
 		return;
-	if (!(inbuf = cs->inbuf)) {
+	inbuf = cs->inbuf;
+	if (!inbuf) {
 		dev_err(cs->dev, "%s: no inbuf\n", __func__);
 		cs_put(cs);
 		return;
@@ -770,18 +784,21 @@ static int __init ser_gigaset_init(void)
 	int rc;
 
 	gig_dbg(DEBUG_INIT, "%s", __func__);
-	if ((rc = platform_driver_register(&device_driver)) != 0) {
+	rc = platform_driver_register(&device_driver);
+	if (rc != 0) {
 		pr_err("error %d registering platform driver\n", rc);
 		return rc;
 	}
 
 	/* allocate memory for our driver state and intialize it */
-	if (!(driver = gigaset_initdriver(GIGASET_MINOR, GIGASET_MINORS,
+	driver = gigaset_initdriver(GIGASET_MINOR, GIGASET_MINORS,
 					  GIGASET_MODULENAME, GIGASET_DEVNAME,
-					  &ops, THIS_MODULE)))
+					  &ops, THIS_MODULE);
+	if (!driver)
 		goto error;
 
-	if ((rc = tty_register_ldisc(N_GIGASET_M101, &gigaset_ldisc)) != 0) {
+	rc = tty_register_ldisc(N_GIGASET_M101, &gigaset_ldisc);
+	if (rc != 0) {
 		pr_err("error %d registering line discipline\n", rc);
 		goto error;
 	}
@@ -808,7 +825,8 @@ static void __exit ser_gigaset_exit(void)
 		driver = NULL;
 	}
 
-	if ((rc = tty_unregister_ldisc(N_GIGASET_M101)) != 0)
+	rc = tty_unregister_ldisc(N_GIGASET_M101);
+	if (rc != 0)
 		pr_err("error %d unregistering line discipline\n", rc);
 
 	platform_driver_unregister(&device_driver);
diff --git a/drivers/isdn/gigaset/usb-gigaset.c b/drivers/isdn/gigaset/usb-gigaset.c
index 4deb1ab0dbf8..3ab1daeb276b 100644
--- a/drivers/isdn/gigaset/usb-gigaset.c
+++ b/drivers/isdn/gigaset/usb-gigaset.c
@@ -43,14 +43,14 @@ MODULE_PARM_DESC(cidmode, "Call-ID mode");
 #define GIGASET_MODULENAME "usb_gigaset"
 #define GIGASET_DEVNAME    "ttyGU"
 
-#define IF_WRITEBUF 2000 //FIXME  // WAKEUP_CHARS: 256
+#define IF_WRITEBUF 2000	/* arbitrary limit */
 
 /* Values for the Gigaset M105 Data */
 #define USB_M105_VENDOR_ID	0x0681
 #define USB_M105_PRODUCT_ID	0x0009
 
 /* table of devices that work with this driver */
-static const struct usb_device_id gigaset_table [] = {
+static const struct usb_device_id gigaset_table[] = {
 	{ USB_DEVICE(USB_M105_VENDOR_ID, USB_M105_PRODUCT_ID) },
 	{ }					/* Terminating entry */
 };
@@ -97,8 +97,8 @@ MODULE_DEVICE_TABLE(usb, gigaset_table);
  *       41 19 -- -- -- -- 06 00 00 00 00 xx 11 13
  *            Used after every "configuration sequence" (RQ 12, RQs 01/03/13).
  *            xx is usually 0x00 but was 0x7e before starting data transfer
- *            in unimodem mode. So, this might be an array of characters that need
- *            special treatment ("commit all bufferd data"?), 11=^Q, 13=^S.
+ *            in unimodem mode. So, this might be an array of characters that
+ *            need special treatment ("commit all bufferd data"?), 11=^Q, 13=^S.
  *
  * Unimodem mode: use "modprobe ppp_async flag_time=0" as the device _needs_ two
  * flags per packet.
@@ -114,7 +114,7 @@ static int gigaset_suspend(struct usb_interface *intf, pm_message_t message);
 static int gigaset_resume(struct usb_interface *intf);
 static int gigaset_pre_reset(struct usb_interface *intf);
 
-static struct gigaset_driver *driver = NULL;
+static struct gigaset_driver *driver;
 
 /* usb specific object needed to register this driver with the usb subsystem */
 static struct usb_driver gigaset_usb_driver = {
@@ -141,6 +141,7 @@ struct usb_cardstate {
 	struct urb		*bulk_out_urb;
 
 	/* Input buffer */
+	unsigned char		*rcvbuf;
 	int			rcvbuf_size;
 	struct urb		*read_urb;
 	__u8			int_in_endpointAddr;
@@ -164,13 +165,11 @@ static int gigaset_set_modem_ctrl(struct cardstate *cs, unsigned old_state,
 	val = tiocm_to_gigaset(new_state);
 
 	gig_dbg(DEBUG_USBREQ, "set flags 0x%02x with mask 0x%02x", val, mask);
-	// don't use this in an interrupt/BH
 	r = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 7, 0x41,
 			    (val & 0xff) | ((mask & 0xff) << 8), 0,
 			    NULL, 0, 2000 /* timeout? */);
 	if (r < 0)
 		return r;
-	//..
 	return 0;
 }
 
@@ -220,7 +219,6 @@ static int gigaset_baud_rate(struct cardstate *cs, unsigned cflag)
 	cflag &= CBAUD;
 
 	switch (cflag) {
-	//FIXME more values?
 	case    B300: rate =     300; break;
 	case    B600: rate =     600; break;
 	case   B1200: rate =    1200; break;
@@ -273,7 +271,7 @@ static int gigaset_set_line_ctrl(struct cardstate *cs, unsigned cflag)
 	/* set the number of stop bits */
 	if (cflag & CSTOPB) {
 		if ((cflag & CSIZE) == CS5)
-			val |= 1; /* 1.5 stop bits */ //FIXME is this okay?
+			val |= 1; /* 1.5 stop bits */
 		else
 			val |= 2; /* 2 stop bits */
 	}
@@ -282,7 +280,7 @@ static int gigaset_set_line_ctrl(struct cardstate *cs, unsigned cflag)
 }
 
 
- /*================================================================================================================*/
+/*============================================================================*/
 static int gigaset_init_bchannel(struct bc_state *bcs)
 {
 	/* nothing to do for M10x */
@@ -344,7 +342,6 @@ static void gigaset_modem_fill(unsigned long data)
 			if (write_modem(cs) < 0) {
 				gig_dbg(DEBUG_OUTPUT,
 					"modem_fill: write_modem failed");
-				// FIXME should we tell the LL?
 				again = 1; /* no callback will be called! */
 			}
 		}
@@ -356,8 +353,8 @@ static void gigaset_modem_fill(unsigned long data)
  */
 static void gigaset_read_int_callback(struct urb *urb)
 {
-	struct inbuf_t *inbuf = urb->context;
-	struct cardstate *cs = inbuf->cs;
+	struct cardstate *cs = urb->context;
+	struct inbuf_t *inbuf = cs->inbuf;
 	int status = urb->status;
 	int r;
 	unsigned numbytes;
@@ -368,7 +365,7 @@ static void gigaset_read_int_callback(struct urb *urb)
 		numbytes = urb->actual_length;
 
 		if (numbytes) {
-			src = inbuf->rcvbuf;
+			src = cs->hw.usb->rcvbuf;
 			if (unlikely(*src))
 				dev_warn(cs->dev,
 				    "%s: There was no leading 0, but 0x%02x!\n",
@@ -440,7 +437,7 @@ static int send_cb(struct cardstate *cs, struct cmdbuf_t *cb)
 	struct cmdbuf_t *tcb;
 	unsigned long flags;
 	int count;
-	int status = -ENOENT; // FIXME
+	int status = -ENOENT;
 	struct usb_cardstate *ucs = cs->hw.usb;
 
 	do {
@@ -480,7 +477,9 @@ static int send_cb(struct cardstate *cs, struct cmdbuf_t *cb)
 			ucs->busy = 1;
 
 			spin_lock_irqsave(&cs->lock, flags);
-			status = cs->connected ? usb_submit_urb(ucs->bulk_out_urb, GFP_ATOMIC) : -ENODEV;
+			status = cs->connected ?
+				usb_submit_urb(ucs->bulk_out_urb, GFP_ATOMIC) :
+				-ENODEV;
 			spin_unlock_irqrestore(&cs->lock, flags);
 
 			if (status) {
@@ -510,8 +509,8 @@ static int gigaset_write_cmd(struct cardstate *cs, const unsigned char *buf,
 
 	if (len <= 0)
 		return 0;
-
-	if (!(cb = kmalloc(sizeof(struct cmdbuf_t) + len, GFP_ATOMIC))) {
+	cb = kmalloc(sizeof(struct cmdbuf_t) + len, GFP_ATOMIC);
+	if (!cb) {
 		dev_err(cs->dev, "%s: out of memory\n", __func__);
 		return -ENOMEM;
 	}
@@ -615,7 +614,7 @@ static int gigaset_initcshw(struct cardstate *cs)
 	ucs->bulk_out_urb = NULL;
 	ucs->read_urb = NULL;
 	tasklet_init(&cs->write_tasklet,
-		     &gigaset_modem_fill, (unsigned long) cs);
+		     gigaset_modem_fill, (unsigned long) cs);
 
 	return 1;
 }
@@ -637,9 +636,7 @@ static int write_modem(struct cardstate *cs)
 		return -EINVAL;
 	}
 
-	/* Copy data to bulk out buffer and  // FIXME copying not necessary
-	 * transmit data
-	 */
+	/* Copy data to bulk out buffer and transmit data */
 	count = min(bcs->tx_skb->len, (unsigned) ucs->bulk_out_size);
 	skb_copy_from_linear_data(bcs->tx_skb, ucs->bulk_out_buffer, count);
 	skb_pull(bcs->tx_skb, count);
@@ -650,7 +647,8 @@ static int write_modem(struct cardstate *cs)
 	if (cs->connected) {
 		usb_fill_bulk_urb(ucs->bulk_out_urb, ucs->udev,
 				  usb_sndbulkpipe(ucs->udev,
-						  ucs->bulk_out_endpointAddr & 0x0f),
+						  ucs->bulk_out_endpointAddr &
+						  0x0f),
 				  ucs->bulk_out_buffer, count,
 				  gigaset_write_bulk_callback, cs);
 		ret = usb_submit_urb(ucs->bulk_out_urb, GFP_ATOMIC);
@@ -666,7 +664,7 @@ static int write_modem(struct cardstate *cs)
 
 	if (!bcs->tx_skb->len) {
 		/* skb sent completely */
-		gigaset_skb_sent(bcs, bcs->tx_skb); //FIXME also, when ret<0?
+		gigaset_skb_sent(bcs, bcs->tx_skb);
 
 		gig_dbg(DEBUG_INTR, "kfree skb (Adr: %lx)!",
 			(unsigned long) bcs->tx_skb);
@@ -763,8 +761,8 @@ static int gigaset_probe(struct usb_interface *interface,
 	buffer_size = le16_to_cpu(endpoint->wMaxPacketSize);
 	ucs->rcvbuf_size = buffer_size;
 	ucs->int_in_endpointAddr = endpoint->bEndpointAddress;
-	cs->inbuf[0].rcvbuf = kmalloc(buffer_size, GFP_KERNEL);
-	if (!cs->inbuf[0].rcvbuf) {
+	ucs->rcvbuf = kmalloc(buffer_size, GFP_KERNEL);
+	if (!ucs->rcvbuf) {
 		dev_err(cs->dev, "Couldn't allocate rcvbuf\n");
 		retval = -ENOMEM;
 		goto error;
@@ -773,9 +771,9 @@ static int gigaset_probe(struct usb_interface *interface,
 	usb_fill_int_urb(ucs->read_urb, udev,
 			 usb_rcvintpipe(udev,
 					endpoint->bEndpointAddress & 0x0f),
-			 cs->inbuf[0].rcvbuf, buffer_size,
+			 ucs->rcvbuf, buffer_size,
 			 gigaset_read_int_callback,
-			 cs->inbuf + 0, endpoint->bInterval);
+			 cs, endpoint->bInterval);
 
 	retval = usb_submit_urb(ucs->read_urb, GFP_KERNEL);
 	if (retval) {
@@ -789,7 +787,7 @@ static int gigaset_probe(struct usb_interface *interface,
 
 	if (!gigaset_start(cs)) {
 		tasklet_kill(&cs->write_tasklet);
-		retval = -ENODEV; //FIXME
+		retval = -ENODEV;
 		goto error;
 	}
 	return 0;
@@ -798,11 +796,11 @@ error:
 	usb_kill_urb(ucs->read_urb);
 	kfree(ucs->bulk_out_buffer);
 	usb_free_urb(ucs->bulk_out_urb);
-	kfree(cs->inbuf[0].rcvbuf);
+	kfree(ucs->rcvbuf);
 	usb_free_urb(ucs->read_urb);
 	usb_set_intfdata(interface, NULL);
 	ucs->read_urb = ucs->bulk_out_urb = NULL;
-	cs->inbuf[0].rcvbuf = ucs->bulk_out_buffer = NULL;
+	ucs->rcvbuf = ucs->bulk_out_buffer = NULL;
 	usb_put_dev(ucs->udev);
 	ucs->udev = NULL;
 	ucs->interface = NULL;
@@ -831,10 +829,10 @@ static void gigaset_disconnect(struct usb_interface *interface)
 
 	kfree(ucs->bulk_out_buffer);
 	usb_free_urb(ucs->bulk_out_urb);
-	kfree(cs->inbuf[0].rcvbuf);
+	kfree(ucs->rcvbuf);
 	usb_free_urb(ucs->read_urb);
 	ucs->read_urb = ucs->bulk_out_urb = NULL;
-	cs->inbuf[0].rcvbuf = ucs->bulk_out_buffer = NULL;
+	ucs->rcvbuf = ucs->bulk_out_buffer = NULL;
 
 	usb_put_dev(ucs->udev);
 	ucs->interface = NULL;
@@ -916,9 +914,10 @@ static int __init usb_gigaset_init(void)
 	int result;
 
 	/* allocate memory for our driver state and intialize it */
-	if ((driver = gigaset_initdriver(GIGASET_MINOR, GIGASET_MINORS,
-				       GIGASET_MODULENAME, GIGASET_DEVNAME,
-				       &ops, THIS_MODULE)) == NULL)
+	driver = gigaset_initdriver(GIGASET_MINOR, GIGASET_MINORS,
+				    GIGASET_MODULENAME, GIGASET_DEVNAME,
+				    &ops, THIS_MODULE);
+	if (driver == NULL)
 		goto error;
 
 	/* register this driver with the USB subsystem */
diff --git a/drivers/isdn/hardware/mISDN/speedfax.c b/drivers/isdn/hardware/mISDN/speedfax.c
index ff3a4e290da3..7726afdbb40b 100644
--- a/drivers/isdn/hardware/mISDN/speedfax.c
+++ b/drivers/isdn/hardware/mISDN/speedfax.c
@@ -110,6 +110,7 @@ set_debug(const char *val, struct kernel_param *kp)
 MODULE_AUTHOR("Karsten Keil");
 MODULE_LICENSE("GPL v2");
 MODULE_VERSION(SPEEDFAX_REV);
+MODULE_FIRMWARE("isdn/ISAR.BIN");
 module_param_call(debug, set_debug, param_get_uint, &debug, S_IRUGO | S_IWUSR);
 MODULE_PARM_DESC(debug, "Speedfax debug mask");
 module_param(irqloops, uint, S_IRUGO | S_IWUSR);
diff --git a/drivers/isdn/mISDN/socket.c b/drivers/isdn/mISDN/socket.c
index feb0fa45b664..fcfe17a19a61 100644
--- a/drivers/isdn/mISDN/socket.c
+++ b/drivers/isdn/mISDN/socket.c
@@ -779,7 +779,7 @@ base_sock_create(struct net *net, struct socket *sock, int protocol)
 }
 
 static int
-mISDN_sock_create(struct net *net, struct socket *sock, int proto)
+mISDN_sock_create(struct net *net, struct socket *sock, int proto, int kern)
 {
 	int err = -EPROTONOSUPPORT;
 
@@ -808,8 +808,7 @@ mISDN_sock_create(struct net *net, struct socket *sock, int proto)
 	return err;
 }
 
-static struct
-net_proto_family mISDN_sock_family_ops = {
+static const struct net_proto_family mISDN_sock_family_ops = {
 	.owner  = THIS_MODULE,
 	.family = PF_ISDN,
 	.create = mISDN_sock_create,
diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index df1f86b5c83e..a2ea383105a6 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -249,5 +249,6 @@ config EP93XX_PWM
 source "drivers/misc/c2port/Kconfig"
 source "drivers/misc/eeprom/Kconfig"
 source "drivers/misc/cb710/Kconfig"
+source "drivers/misc/iwmc3200top/Kconfig"
 
 endif # MISC_DEVICES
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index f982d2ecfde7..e311267a355f 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -21,5 +21,6 @@ obj-$(CONFIG_HP_ILO)		+= hpilo.o
 obj-$(CONFIG_ISL29003)		+= isl29003.o
 obj-$(CONFIG_EP93XX_PWM)	+= ep93xx_pwm.o
 obj-$(CONFIG_C2PORT)		+= c2port/
+obj-$(CONFIG_IWMC3200TOP)      += iwmc3200top/
 obj-y				+= eeprom/
 obj-y				+= cb710/
diff --git a/drivers/misc/iwmc3200top/Kconfig b/drivers/misc/iwmc3200top/Kconfig
new file mode 100644
index 000000000000..9e4b88fb57f1
--- /dev/null
+++ b/drivers/misc/iwmc3200top/Kconfig
@@ -0,0 +1,20 @@
+config IWMC3200TOP
+        tristate "Intel Wireless MultiCom Top Driver"
+        depends on MMC && EXPERIMENTAL
+        select FW_LOADER
+	---help---
+	  Intel Wireless MultiCom 3200 Top driver is responsible for
+	  for firmware load and enabled coms enumeration
+
+config IWMC3200TOP_DEBUG
+	bool "Enable full debug output of iwmc3200top Driver"
+	depends on IWMC3200TOP
+	---help---
+	  Enable full debug output of iwmc3200top Driver
+
+config IWMC3200TOP_DEBUGFS
+	bool "Enable Debugfs debugging interface for iwmc3200top"
+	depends on IWMC3200TOP
+	---help---
+	  Enable creation of debugfs files for iwmc3200top
+
diff --git a/drivers/misc/iwmc3200top/Makefile b/drivers/misc/iwmc3200top/Makefile
new file mode 100644
index 000000000000..fbf53fb4634e
--- /dev/null
+++ b/drivers/misc/iwmc3200top/Makefile
@@ -0,0 +1,29 @@
+# iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+# drivers/misc/iwmc3200top/Makefile
+#
+# Copyright (C) 2009 Intel Corporation. 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.
+#
+# This program is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+# GNU General Public License for more details.
+#
+# You 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.
+#
+#
+# Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+#  -
+#
+#
+
+obj-$(CONFIG_IWMC3200TOP)	+= iwmc3200top.o
+iwmc3200top-objs	:= main.o fw-download.o
+iwmc3200top-$(CONFIG_IWMC3200TOP_DEBUG) += log.o
+iwmc3200top-$(CONFIG_IWMC3200TOP_DEBUGFS) += debugfs.o
diff --git a/drivers/misc/iwmc3200top/debugfs.c b/drivers/misc/iwmc3200top/debugfs.c
new file mode 100644
index 000000000000..0c8ea0a1c8a3
--- /dev/null
+++ b/drivers/misc/iwmc3200top/debugfs.c
@@ -0,0 +1,133 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/debufs.c
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/ctype.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio.h>
+#include <linux/debugfs.h>
+
+#include "iwmc3200top.h"
+#include "fw-msg.h"
+#include "log.h"
+#include "debugfs.h"
+
+
+
+/*      Constants definition        */
+#define HEXADECIMAL_RADIX	16
+
+/*      Functions definition        */
+
+
+#define DEBUGFS_ADD(name, parent) do {					\
+	dbgfs->dbgfs_##parent##_files.file_##name =			\
+	debugfs_create_file(#name, 0644, dbgfs->dir_##parent, priv,	\
+				&iwmct_dbgfs_##name##_ops);		\
+} while (0)
+
+#define DEBUGFS_RM(name)  do {		\
+	debugfs_remove(name);		\
+	name = NULL;			\
+} while (0)
+
+#define DEBUGFS_READ_FUNC(name)						\
+ssize_t iwmct_dbgfs_##name##_read(struct file *file,			\
+				  char __user *user_buf,		\
+				  size_t count, loff_t *ppos);
+
+#define DEBUGFS_WRITE_FUNC(name)					\
+ssize_t iwmct_dbgfs_##name##_write(struct file *file,			\
+				   const char __user *user_buf,		\
+				   size_t count, loff_t *ppos);
+
+#define DEBUGFS_READ_FILE_OPS(name)					\
+	DEBUGFS_READ_FUNC(name)						\
+	static const struct file_operations iwmct_dbgfs_##name##_ops = {  \
+		.read = iwmct_dbgfs_##name##_read,			\
+		.open = iwmct_dbgfs_open_file_generic,			\
+	};
+
+#define DEBUGFS_WRITE_FILE_OPS(name)					\
+	DEBUGFS_WRITE_FUNC(name)					\
+	static const struct file_operations iwmct_dbgfs_##name##_ops = {  \
+		.write = iwmct_dbgfs_##name##_write,			\
+		.open = iwmct_dbgfs_open_file_generic,			\
+	};
+
+#define DEBUGFS_READ_WRITE_FILE_OPS(name)				\
+	DEBUGFS_READ_FUNC(name)						\
+	DEBUGFS_WRITE_FUNC(name)					\
+	static const struct file_operations iwmct_dbgfs_##name##_ops = {\
+		.write = iwmct_dbgfs_##name##_write,			\
+		.read = iwmct_dbgfs_##name##_read,			\
+		.open = iwmct_dbgfs_open_file_generic,			\
+	};
+
+
+/*      Debugfs file ops definitions        */
+
+/*
+ * Create the debugfs files and directories
+ *
+ */
+void iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name)
+{
+	struct iwmct_debugfs *dbgfs;
+
+	dbgfs = kzalloc(sizeof(struct iwmct_debugfs), GFP_KERNEL);
+	if (!dbgfs) {
+		LOG_ERROR(priv, DEBUGFS, "failed to allocate %zd bytes\n",
+					sizeof(struct iwmct_debugfs));
+		return;
+	}
+
+	priv->dbgfs = dbgfs;
+	dbgfs->name = name;
+	dbgfs->dir_drv = debugfs_create_dir(name, NULL);
+	if (!dbgfs->dir_drv) {
+		LOG_ERROR(priv, DEBUGFS, "failed to create debugfs dir\n");
+		return;
+	}
+
+	return;
+}
+
+/**
+ * Remove the debugfs files and directories
+ *
+ */
+void iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs)
+{
+	if (!dbgfs)
+		return;
+
+	DEBUGFS_RM(dbgfs->dir_drv);
+	kfree(dbgfs);
+	dbgfs = NULL;
+}
+
diff --git a/drivers/misc/iwmc3200top/debugfs.h b/drivers/misc/iwmc3200top/debugfs.h
new file mode 100644
index 000000000000..71d45759b40f
--- /dev/null
+++ b/drivers/misc/iwmc3200top/debugfs.h
@@ -0,0 +1,58 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/debufs.h
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __DEBUGFS_H__
+#define __DEBUGFS_H__
+
+
+#ifdef CONFIG_IWMC3200TOP_DEBUGFS
+
+struct iwmct_debugfs {
+	const char *name;
+	struct dentry *dir_drv;
+	struct dir_drv_files {
+	} dbgfs_drv_files;
+};
+
+void iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name);
+void iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs);
+
+#else /* CONFIG_IWMC3200TOP_DEBUGFS */
+
+struct iwmct_debugfs;
+
+static inline void
+iwmct_dbgfs_register(struct iwmct_priv *priv, const char *name)
+{}
+
+static inline void
+iwmct_dbgfs_unregister(struct iwmct_debugfs *dbgfs)
+{}
+
+#endif /* CONFIG_IWMC3200TOP_DEBUGFS */
+
+#endif /* __DEBUGFS_H__ */
+
diff --git a/drivers/misc/iwmc3200top/fw-download.c b/drivers/misc/iwmc3200top/fw-download.c
new file mode 100644
index 000000000000..50d431e469f5
--- /dev/null
+++ b/drivers/misc/iwmc3200top/fw-download.c
@@ -0,0 +1,355 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/fw-download.c
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/firmware.h>
+#include <linux/mmc/sdio_func.h>
+#include <asm/unaligned.h>
+
+#include "iwmc3200top.h"
+#include "log.h"
+#include "fw-msg.h"
+
+#define CHECKSUM_BYTES_NUM sizeof(u32)
+
+/**
+  init parser struct with file
+ */
+static int iwmct_fw_parser_init(struct iwmct_priv *priv, const u8 *file,
+			      size_t file_size, size_t block_size)
+{
+	struct iwmct_parser *parser = &priv->parser;
+	struct iwmct_fw_hdr *fw_hdr = &parser->versions;
+
+	LOG_INFOEX(priv, INIT, "-->\n");
+
+	LOG_INFO(priv, FW_DOWNLOAD, "file_size=%zd\n", file_size);
+
+	parser->file = file;
+	parser->file_size = file_size;
+	parser->cur_pos = 0;
+	parser->buf = NULL;
+
+	parser->buf = kzalloc(block_size, GFP_KERNEL);
+	if (!parser->buf) {
+		LOG_ERROR(priv, FW_DOWNLOAD, "kzalloc error\n");
+		return -ENOMEM;
+	}
+	parser->buf_size = block_size;
+
+	/* extract fw versions */
+	memcpy(fw_hdr, parser->file, sizeof(struct iwmct_fw_hdr));
+	LOG_INFO(priv, FW_DOWNLOAD, "fw versions are:\n"
+		"top %u.%u.%u gps %u.%u.%u bt %u.%u.%u tic %s\n",
+		fw_hdr->top_major, fw_hdr->top_minor, fw_hdr->top_revision,
+		fw_hdr->gps_major, fw_hdr->gps_minor, fw_hdr->gps_revision,
+		fw_hdr->bt_major, fw_hdr->bt_minor, fw_hdr->bt_revision,
+		fw_hdr->tic_name);
+
+	parser->cur_pos += sizeof(struct iwmct_fw_hdr);
+
+	LOG_INFOEX(priv, INIT, "<--\n");
+	return 0;
+}
+
+static bool iwmct_checksum(struct iwmct_priv *priv)
+{
+	struct iwmct_parser *parser = &priv->parser;
+	__le32 *file = (__le32 *)parser->file;
+	int i, pad, steps;
+	u32 accum = 0;
+	u32 checksum;
+	u32 mask = 0xffffffff;
+
+	pad = (parser->file_size - CHECKSUM_BYTES_NUM) % 4;
+	steps =  (parser->file_size - CHECKSUM_BYTES_NUM) / 4;
+
+	LOG_INFO(priv, FW_DOWNLOAD, "pad=%d steps=%d\n", pad, steps);
+
+	for (i = 0; i < steps; i++)
+		accum += le32_to_cpu(file[i]);
+
+	if (pad) {
+		mask <<= 8 * (4 - pad);
+		accum += le32_to_cpu(file[steps]) & mask;
+	}
+
+	checksum = get_unaligned_le32((__le32 *)(parser->file +
+			parser->file_size - CHECKSUM_BYTES_NUM));
+
+	LOG_INFO(priv, FW_DOWNLOAD,
+		"compare checksum accum=0x%x to checksum=0x%x\n",
+		accum, checksum);
+
+	return checksum == accum;
+}
+
+static int iwmct_parse_next_section(struct iwmct_priv *priv, const u8 **p_sec,
+				  size_t *sec_size, __le32 *sec_addr)
+{
+	struct iwmct_parser *parser = &priv->parser;
+	struct iwmct_dbg *dbg = &priv->dbg;
+	struct iwmct_fw_sec_hdr *sec_hdr;
+
+	LOG_INFOEX(priv, INIT, "-->\n");
+
+	while (parser->cur_pos + sizeof(struct iwmct_fw_sec_hdr)
+		<= parser->file_size) {
+
+		sec_hdr = (struct iwmct_fw_sec_hdr *)
+				(parser->file + parser->cur_pos);
+		parser->cur_pos += sizeof(struct iwmct_fw_sec_hdr);
+
+		LOG_INFO(priv, FW_DOWNLOAD,
+			"sec hdr: type=%s addr=0x%x size=%d\n",
+			sec_hdr->type, sec_hdr->target_addr,
+			sec_hdr->data_size);
+
+		if (strcmp(sec_hdr->type, "ENT") == 0)
+			parser->entry_point = le32_to_cpu(sec_hdr->target_addr);
+		else if (strcmp(sec_hdr->type, "LBL") == 0)
+			strcpy(dbg->label_fw, parser->file + parser->cur_pos);
+		else if (((strcmp(sec_hdr->type, "TOP") == 0) &&
+			  (priv->barker & BARKER_DNLOAD_TOP_MSK)) ||
+			 ((strcmp(sec_hdr->type, "GPS") == 0) &&
+			  (priv->barker & BARKER_DNLOAD_GPS_MSK)) ||
+			 ((strcmp(sec_hdr->type, "BTH") == 0) &&
+			  (priv->barker & BARKER_DNLOAD_BT_MSK))) {
+			*sec_addr = sec_hdr->target_addr;
+			*sec_size = le32_to_cpu(sec_hdr->data_size);
+			*p_sec = parser->file + parser->cur_pos;
+			parser->cur_pos += le32_to_cpu(sec_hdr->data_size);
+			return 1;
+		} else if (strcmp(sec_hdr->type, "LOG") != 0)
+			LOG_WARNING(priv, FW_DOWNLOAD,
+				    "skipping section type %s\n",
+				    sec_hdr->type);
+
+		parser->cur_pos += le32_to_cpu(sec_hdr->data_size);
+		LOG_INFO(priv, FW_DOWNLOAD,
+			"finished with section cur_pos=%zd\n", parser->cur_pos);
+	}
+
+	LOG_INFOEX(priv, INIT, "<--\n");
+	return 0;
+}
+
+static int iwmct_download_section(struct iwmct_priv *priv, const u8 *p_sec,
+				size_t sec_size, __le32 addr)
+{
+	struct iwmct_parser *parser = &priv->parser;
+	struct iwmct_fw_load_hdr *hdr = (struct iwmct_fw_load_hdr *)parser->buf;
+	const u8 *cur_block = p_sec;
+	size_t sent = 0;
+	int cnt = 0;
+	int ret = 0;
+	u32 cmd = 0;
+
+	LOG_INFOEX(priv, INIT, "-->\n");
+	LOG_INFO(priv, FW_DOWNLOAD, "Download address 0x%x size 0x%zx\n",
+				addr, sec_size);
+
+	while (sent < sec_size) {
+		int i;
+		u32 chksm = 0;
+		u32 reset = atomic_read(&priv->reset);
+		/* actual FW data */
+		u32 data_size = min(parser->buf_size - sizeof(*hdr),
+				    sec_size - sent);
+		/* Pad to block size */
+		u32 trans_size = (data_size + sizeof(*hdr) +
+				  IWMC_SDIO_BLK_SIZE - 1) &
+				  ~(IWMC_SDIO_BLK_SIZE - 1);
+		++cnt;
+
+		/* in case of reset, interrupt FW DOWNLAOD */
+		if (reset) {
+			LOG_INFO(priv, FW_DOWNLOAD,
+				 "Reset detected. Abort FW download!!!");
+			ret = -ECANCELED;
+			goto exit;
+		}
+
+		memset(parser->buf, 0, parser->buf_size);
+		cmd |= IWMC_OPCODE_WRITE << CMD_HDR_OPCODE_POS;
+		cmd |= IWMC_CMD_SIGNATURE << CMD_HDR_SIGNATURE_POS;
+		cmd |= (priv->dbg.direct ? 1 : 0) << CMD_HDR_DIRECT_ACCESS_POS;
+		cmd |= (priv->dbg.checksum ? 1 : 0) << CMD_HDR_USE_CHECKSUM_POS;
+		hdr->data_size = cpu_to_le32(data_size);
+		hdr->target_addr = addr;
+
+		/* checksum is allowed for sizes divisible by 4 */
+		if (data_size & 0x3)
+			cmd &= ~CMD_HDR_USE_CHECKSUM_MSK;
+
+		memcpy(hdr->data, cur_block, data_size);
+
+
+		if (cmd & CMD_HDR_USE_CHECKSUM_MSK) {
+
+			chksm = data_size + le32_to_cpu(addr) + cmd;
+			for (i = 0; i < data_size >> 2; i++)
+				chksm += ((u32 *)cur_block)[i];
+
+			hdr->block_chksm = cpu_to_le32(chksm);
+			LOG_INFO(priv, FW_DOWNLOAD, "Checksum = 0x%X\n",
+				 hdr->block_chksm);
+		}
+
+		LOG_INFO(priv, FW_DOWNLOAD, "trans#%d, len=%d, sent=%zd, "
+				"sec_size=%zd, startAddress 0x%X\n",
+				cnt, trans_size, sent, sec_size, addr);
+
+		if (priv->dbg.dump)
+			LOG_HEXDUMP(FW_DOWNLOAD, parser->buf, trans_size);
+
+
+		hdr->cmd = cpu_to_le32(cmd);
+		/* send it down */
+		/* TODO: add more proper sending and error checking */
+		ret = iwmct_tx(priv, 0, parser->buf, trans_size);
+		if (ret != 0) {
+			LOG_INFO(priv, FW_DOWNLOAD,
+				"iwmct_tx returned %d\n", ret);
+			goto exit;
+		}
+
+		addr = cpu_to_le32(le32_to_cpu(addr) + data_size);
+		sent += data_size;
+		cur_block = p_sec + sent;
+
+		if (priv->dbg.blocks && (cnt + 1) >= priv->dbg.blocks) {
+			LOG_INFO(priv, FW_DOWNLOAD,
+				"Block number limit is reached [%d]\n",
+				priv->dbg.blocks);
+			break;
+		}
+	}
+
+	if (sent < sec_size)
+		ret = -EINVAL;
+exit:
+	LOG_INFOEX(priv, INIT, "<--\n");
+	return ret;
+}
+
+static int iwmct_kick_fw(struct iwmct_priv *priv, bool jump)
+{
+	struct iwmct_parser *parser = &priv->parser;
+	struct iwmct_fw_load_hdr *hdr = (struct iwmct_fw_load_hdr *)parser->buf;
+	int ret;
+	u32 cmd;
+
+	LOG_INFOEX(priv, INIT, "-->\n");
+
+	memset(parser->buf, 0, parser->buf_size);
+	cmd = IWMC_CMD_SIGNATURE << CMD_HDR_SIGNATURE_POS;
+	if (jump) {
+		cmd |= IWMC_OPCODE_JUMP << CMD_HDR_OPCODE_POS;
+		hdr->target_addr = cpu_to_le32(parser->entry_point);
+		LOG_INFO(priv, FW_DOWNLOAD, "jump address 0x%x\n",
+				parser->entry_point);
+	} else {
+		cmd |= IWMC_OPCODE_LAST_COMMAND << CMD_HDR_OPCODE_POS;
+		LOG_INFO(priv, FW_DOWNLOAD, "last command\n");
+	}
+
+	hdr->cmd = cpu_to_le32(cmd);
+
+	LOG_HEXDUMP(FW_DOWNLOAD, parser->buf, sizeof(*hdr));
+	/* send it down */
+	/* TODO: add more proper sending and error checking */
+	ret = iwmct_tx(priv, 0, parser->buf, IWMC_SDIO_BLK_SIZE);
+	if (ret)
+		LOG_INFO(priv, FW_DOWNLOAD, "iwmct_tx returned %d", ret);
+
+	LOG_INFOEX(priv, INIT, "<--\n");
+	return 0;
+}
+
+int iwmct_fw_load(struct iwmct_priv *priv)
+{
+	const u8 *fw_name = FW_NAME(FW_API_VER);
+	const struct firmware *raw;
+	const u8 *pdata;
+	size_t len;
+	__le32 addr;
+	int ret;
+
+	/* clear parser struct */
+	memset(&priv->parser, 0, sizeof(struct iwmct_parser));
+
+	/* get the firmware */
+	ret = request_firmware(&raw, fw_name, &priv->func->dev);
+	if (ret < 0) {
+		LOG_ERROR(priv, FW_DOWNLOAD, "%s request_firmware failed %d\n",
+			  fw_name, ret);
+		goto exit;
+	}
+
+	if (raw->size < sizeof(struct iwmct_fw_sec_hdr)) {
+		LOG_ERROR(priv, FW_DOWNLOAD, "%s smaller then (%zd) (%zd)\n",
+			  fw_name, sizeof(struct iwmct_fw_sec_hdr), raw->size);
+		goto exit;
+	}
+
+	LOG_INFO(priv, FW_DOWNLOAD, "Read firmware '%s'\n", fw_name);
+
+	ret = iwmct_fw_parser_init(priv, raw->data, raw->size, priv->trans_len);
+	if (ret < 0) {
+		LOG_ERROR(priv, FW_DOWNLOAD,
+			  "iwmct_parser_init failed: Reason %d\n", ret);
+		goto exit;
+	}
+
+	/* checksum  */
+	if (!iwmct_checksum(priv)) {
+		LOG_ERROR(priv, FW_DOWNLOAD, "checksum error\n");
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	/* download firmware to device */
+	while (iwmct_parse_next_section(priv, &pdata, &len, &addr)) {
+		if (iwmct_download_section(priv, pdata, len, addr)) {
+			LOG_ERROR(priv, FW_DOWNLOAD,
+				  "%s download section failed\n", fw_name);
+			ret = -EIO;
+			goto exit;
+		}
+	}
+
+	iwmct_kick_fw(priv, !!(priv->barker & BARKER_DNLOAD_JUMP_MSK));
+
+exit:
+	kfree(priv->parser.buf);
+
+	if (raw)
+		release_firmware(raw);
+
+	raw = NULL;
+
+	return ret;
+}
diff --git a/drivers/misc/iwmc3200top/fw-msg.h b/drivers/misc/iwmc3200top/fw-msg.h
new file mode 100644
index 000000000000..9e26b75bd482
--- /dev/null
+++ b/drivers/misc/iwmc3200top/fw-msg.h
@@ -0,0 +1,113 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/fw-msg.h
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __FWMSG_H__
+#define __FWMSG_H__
+
+#define COMM_TYPE_D2H           	0xFF
+#define COMM_TYPE_H2D           	0xEE
+
+#define COMM_CATEGORY_OPERATIONAL      	0x00
+#define COMM_CATEGORY_DEBUG            	0x01
+#define COMM_CATEGORY_TESTABILITY      	0x02
+#define COMM_CATEGORY_DIAGNOSTICS      	0x03
+
+#define OP_DBG_ZSTR_MSG			cpu_to_le16(0x1A)
+
+#define FW_LOG_SRC_MAX			32
+#define FW_LOG_SRC_ALL			255
+
+#define FW_STRING_TABLE_ADDR		cpu_to_le32(0x0C000000)
+
+#define CMD_DBG_LOG_LEVEL		cpu_to_le16(0x0001)
+#define CMD_TST_DEV_RESET		cpu_to_le16(0x0060)
+#define CMD_TST_FUNC_RESET		cpu_to_le16(0x0062)
+#define CMD_TST_IFACE_RESET		cpu_to_le16(0x0064)
+#define CMD_TST_CPU_UTILIZATION		cpu_to_le16(0x0065)
+#define CMD_TST_TOP_DEEP_SLEEP		cpu_to_le16(0x0080)
+#define CMD_TST_WAKEUP			cpu_to_le16(0x0081)
+#define CMD_TST_FUNC_WAKEUP		cpu_to_le16(0x0082)
+#define CMD_TST_FUNC_DEEP_SLEEP_REQUEST	cpu_to_le16(0x0083)
+#define CMD_TST_GET_MEM_DUMP		cpu_to_le16(0x0096)
+
+#define OP_OPR_ALIVE			cpu_to_le16(0x0010)
+#define OP_OPR_CMD_ACK			cpu_to_le16(0x001F)
+#define OP_OPR_CMD_NACK			cpu_to_le16(0x0020)
+#define OP_TST_MEM_DUMP			cpu_to_le16(0x0043)
+
+#define CMD_FLAG_PADDING_256		0x80
+
+#define FW_HCMD_BLOCK_SIZE      	256
+
+struct msg_hdr {
+	u8 type;
+	u8 category;
+	__le16 opcode;
+	u8 seqnum;
+	u8 flags;
+	__le16 length;
+} __attribute__((__packed__));
+
+struct log_hdr {
+	__le32 timestamp;
+	u8 severity;
+	u8 logsource;
+	__le16 reserved;
+} __attribute__((__packed__));
+
+struct mdump_hdr {
+	u8 dmpid;
+	u8 frag;
+	__le16 size;
+	__le32 addr;
+} __attribute__((__packed__));
+
+struct top_msg {
+	struct msg_hdr hdr;
+	union {
+		/* D2H messages */
+		struct {
+			struct log_hdr log_hdr;
+			u8 data[1];
+		} __attribute__((__packed__)) log;
+
+		struct {
+			struct log_hdr log_hdr;
+			struct mdump_hdr md_hdr;
+			u8 data[1];
+		} __attribute__((__packed__)) mdump;
+
+		/* H2D messages */
+		struct {
+			u8 logsource;
+			u8 sevmask;
+		} __attribute__((__packed__)) logdefs[FW_LOG_SRC_MAX];
+		struct mdump_hdr mdump_req;
+	} u;
+} __attribute__((__packed__));
+
+
+#endif /* __FWMSG_H__ */
diff --git a/drivers/misc/iwmc3200top/iwmc3200top.h b/drivers/misc/iwmc3200top/iwmc3200top.h
new file mode 100644
index 000000000000..43bd510e1872
--- /dev/null
+++ b/drivers/misc/iwmc3200top/iwmc3200top.h
@@ -0,0 +1,209 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/iwmc3200top.h
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __IWMC3200TOP_H__
+#define __IWMC3200TOP_H__
+
+#include <linux/workqueue.h>
+
+#define DRV_NAME "iwmc3200top"
+#define FW_API_VER 1
+#define _FW_NAME(api) DRV_NAME "." #api ".fw"
+#define FW_NAME(api) _FW_NAME(api)
+
+#define IWMC_SDIO_BLK_SIZE			256
+#define IWMC_DEFAULT_TR_BLK			64
+#define IWMC_SDIO_DATA_ADDR			0x0
+#define IWMC_SDIO_INTR_ENABLE_ADDR		0x14
+#define IWMC_SDIO_INTR_STATUS_ADDR		0x13
+#define IWMC_SDIO_INTR_CLEAR_ADDR		0x13
+#define IWMC_SDIO_INTR_GET_SIZE_ADDR		0x2C
+
+#define COMM_HUB_HEADER_LENGTH 16
+#define LOGGER_HEADER_LENGTH   10
+
+
+#define BARKER_DNLOAD_BT_POS		0
+#define BARKER_DNLOAD_BT_MSK		BIT(BARKER_DNLOAD_BT_POS)
+#define BARKER_DNLOAD_GPS_POS		1
+#define BARKER_DNLOAD_GPS_MSK		BIT(BARKER_DNLOAD_GPS_POS)
+#define BARKER_DNLOAD_TOP_POS		2
+#define BARKER_DNLOAD_TOP_MSK		BIT(BARKER_DNLOAD_TOP_POS)
+#define BARKER_DNLOAD_RESERVED1_POS	3
+#define BARKER_DNLOAD_RESERVED1_MSK	BIT(BARKER_DNLOAD_RESERVED1_POS)
+#define BARKER_DNLOAD_JUMP_POS		4
+#define BARKER_DNLOAD_JUMP_MSK		BIT(BARKER_DNLOAD_JUMP_POS)
+#define BARKER_DNLOAD_SYNC_POS		5
+#define BARKER_DNLOAD_SYNC_MSK		BIT(BARKER_DNLOAD_SYNC_POS)
+#define BARKER_DNLOAD_RESERVED2_POS	6
+#define BARKER_DNLOAD_RESERVED2_MSK	(0x3 << BARKER_DNLOAD_RESERVED2_POS)
+#define BARKER_DNLOAD_BARKER_POS	8
+#define BARKER_DNLOAD_BARKER_MSK	(0xffffff << BARKER_DNLOAD_BARKER_POS)
+
+#define IWMC_BARKER_REBOOT 	(0xdeadbe << BARKER_DNLOAD_BARKER_POS)
+/* whole field barker */
+#define IWMC_BARKER_ACK 	0xfeedbabe
+
+#define IWMC_CMD_SIGNATURE 	0xcbbc
+
+#define CMD_HDR_OPCODE_POS		0
+#define CMD_HDR_OPCODE_MSK_MSK		(0xf << CMD_HDR_OPCODE_MSK_POS)
+#define CMD_HDR_RESPONSE_CODE_POS	4
+#define CMD_HDR_RESPONSE_CODE_MSK	(0xf << CMD_HDR_RESPONSE_CODE_POS)
+#define CMD_HDR_USE_CHECKSUM_POS	8
+#define CMD_HDR_USE_CHECKSUM_MSK	BIT(CMD_HDR_USE_CHECKSUM_POS)
+#define CMD_HDR_RESPONSE_REQUIRED_POS	9
+#define CMD_HDR_RESPONSE_REQUIRED_MSK	BIT(CMD_HDR_RESPONSE_REQUIRED_POS)
+#define CMD_HDR_DIRECT_ACCESS_POS	10
+#define CMD_HDR_DIRECT_ACCESS_MSK	BIT(CMD_HDR_DIRECT_ACCESS_POS)
+#define CMD_HDR_RESERVED_POS		11
+#define CMD_HDR_RESERVED_MSK		BIT(0x1f << CMD_HDR_RESERVED_POS)
+#define CMD_HDR_SIGNATURE_POS		16
+#define CMD_HDR_SIGNATURE_MSK		BIT(0xffff << CMD_HDR_SIGNATURE_POS)
+
+enum {
+	IWMC_OPCODE_PING = 0,
+	IWMC_OPCODE_READ = 1,
+	IWMC_OPCODE_WRITE = 2,
+	IWMC_OPCODE_JUMP = 3,
+	IWMC_OPCODE_REBOOT = 4,
+	IWMC_OPCODE_PERSISTENT_WRITE = 5,
+	IWMC_OPCODE_PERSISTENT_READ = 6,
+	IWMC_OPCODE_READ_MODIFY_WRITE = 7,
+	IWMC_OPCODE_LAST_COMMAND = 15
+};
+
+struct iwmct_fw_load_hdr {
+	__le32 cmd;
+	__le32 target_addr;
+	__le32 data_size;
+	__le32 block_chksm;
+	u8 data[0];
+};
+
+/**
+ * struct iwmct_fw_hdr
+ * holds all sw components versions
+ */
+struct iwmct_fw_hdr {
+	u8 top_major;
+	u8 top_minor;
+	u8 top_revision;
+	u8 gps_major;
+	u8 gps_minor;
+	u8 gps_revision;
+	u8 bt_major;
+	u8 bt_minor;
+	u8 bt_revision;
+	u8 tic_name[31];
+};
+
+/**
+ * struct iwmct_fw_sec_hdr
+ * @type: function type
+ * @data_size: section's data size
+ * @target_addr: download address
+ */
+struct iwmct_fw_sec_hdr {
+	u8 type[4];
+	__le32 data_size;
+	__le32 target_addr;
+};
+
+/**
+ * struct iwmct_parser
+ * @file: fw image
+ * @file_size: fw size
+ * @cur_pos: position in file
+ * @buf: temp buf for download
+ * @buf_size: size of buf
+ * @entry_point: address to jump in fw kick-off
+ */
+struct iwmct_parser {
+	const u8 *file;
+	size_t file_size;
+	size_t cur_pos;
+	u8 *buf;
+	size_t buf_size;
+	u32 entry_point;
+	struct iwmct_fw_hdr versions;
+};
+
+
+struct iwmct_work_struct {
+	struct list_head list;
+	ssize_t iosize;
+};
+
+struct iwmct_dbg {
+	int blocks;
+	bool dump;
+	bool jump;
+	bool direct;
+	bool checksum;
+	bool fw_download;
+	int block_size;
+	int download_trans_blks;
+
+	char label_fw[256];
+};
+
+struct iwmct_debugfs;
+
+struct iwmct_priv {
+	struct sdio_func *func;
+	struct iwmct_debugfs *dbgfs;
+	struct iwmct_parser parser;
+	atomic_t reset;
+	atomic_t dev_sync;
+	u32 trans_len;
+	u32 barker;
+	struct iwmct_dbg dbg;
+
+	/* drivers work queue */
+	struct workqueue_struct *wq;
+	struct workqueue_struct *bus_rescan_wq;
+	struct work_struct bus_rescan_worker;
+	struct work_struct isr_worker;
+
+	/* drivers wait queue */
+	wait_queue_head_t wait_q;
+
+	/* rx request list */
+	struct list_head read_req_list;
+};
+
+extern int iwmct_tx(struct iwmct_priv *priv, unsigned int addr,
+		void *src, int count);
+
+extern int iwmct_fw_load(struct iwmct_priv *priv);
+
+extern void iwmct_dbg_init_params(struct iwmct_priv *drv);
+extern void iwmct_dbg_init_drv_attrs(struct device_driver *drv);
+extern void iwmct_dbg_remove_drv_attrs(struct device_driver *drv);
+extern int iwmct_send_hcmd(struct iwmct_priv *priv, u8 *cmd, u16 len);
+
+#endif  /*  __IWMC3200TOP_H__  */
diff --git a/drivers/misc/iwmc3200top/log.c b/drivers/misc/iwmc3200top/log.c
new file mode 100644
index 000000000000..d569279698f6
--- /dev/null
+++ b/drivers/misc/iwmc3200top/log.c
@@ -0,0 +1,347 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/log.c
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/ctype.h>
+#include "fw-msg.h"
+#include "iwmc3200top.h"
+#include "log.h"
+
+/* Maximal hexadecimal string size of the FW memdump message */
+#define LOG_MSG_SIZE_MAX		12400
+
+/* iwmct_logdefs is a global used by log macros */
+u8 iwmct_logdefs[LOG_SRC_MAX];
+static u8 iwmct_fw_logdefs[FW_LOG_SRC_MAX];
+
+
+static int _log_set_log_filter(u8 *logdefs, int size, u8 src, u8 logmask)
+{
+	int i;
+
+	if (src < size)
+		logdefs[src] = logmask;
+	else if (src == LOG_SRC_ALL)
+		for (i = 0; i < size; i++)
+			logdefs[i] = logmask;
+	else
+		return -1;
+
+	return 0;
+}
+
+
+int iwmct_log_set_filter(u8 src, u8 logmask)
+{
+	return _log_set_log_filter(iwmct_logdefs, LOG_SRC_MAX, src, logmask);
+}
+
+
+int iwmct_log_set_fw_filter(u8 src, u8 logmask)
+{
+	return _log_set_log_filter(iwmct_fw_logdefs,
+				   FW_LOG_SRC_MAX, src, logmask);
+}
+
+
+static int log_msg_format_hex(char *str, int slen, u8 *ibuf,
+			      int ilen, char *pref)
+{
+	int pos = 0;
+	int i;
+	int len;
+
+	for (pos = 0, i = 0; pos < slen - 2 && pref[i] != '\0'; i++, pos++)
+		str[pos] = pref[i];
+
+	for (i = 0; pos < slen - 2 && i < ilen; pos += len, i++)
+		len = snprintf(&str[pos], slen - pos - 1, " %2.2X", ibuf[i]);
+
+	if (i < ilen)
+		return -1;
+
+	return 0;
+}
+
+/*	NOTE: This function is not thread safe.
+	Currently it's called only from sdio rx worker - no race there
+*/
+void iwmct_log_top_message(struct iwmct_priv *priv, u8 *buf, int len)
+{
+	struct top_msg *msg;
+	static char logbuf[LOG_MSG_SIZE_MAX];
+
+	msg = (struct top_msg *)buf;
+
+	if (len < sizeof(msg->hdr) + sizeof(msg->u.log.log_hdr)) {
+		LOG_ERROR(priv, FW_MSG, "Log message from TOP "
+			  "is too short %d (expected %zd)\n",
+			  len, sizeof(msg->hdr) + sizeof(msg->u.log.log_hdr));
+		return;
+	}
+
+	if (!(iwmct_fw_logdefs[msg->u.log.log_hdr.logsource] &
+		BIT(msg->u.log.log_hdr.severity)) ||
+	    !(iwmct_logdefs[LOG_SRC_FW_MSG] & BIT(msg->u.log.log_hdr.severity)))
+		return;
+
+	switch (msg->hdr.category) {
+	case COMM_CATEGORY_TESTABILITY:
+		if (!(iwmct_logdefs[LOG_SRC_TST] &
+		      BIT(msg->u.log.log_hdr.severity)))
+			return;
+		if (log_msg_format_hex(logbuf, LOG_MSG_SIZE_MAX, buf,
+				       le16_to_cpu(msg->hdr.length) +
+				       sizeof(msg->hdr), "<TST>"))
+			LOG_WARNING(priv, TST,
+				  "TOP TST message is too long, truncating...");
+		LOG_WARNING(priv, TST, "%s\n", logbuf);
+		break;
+	case COMM_CATEGORY_DEBUG:
+		if (msg->hdr.opcode == OP_DBG_ZSTR_MSG)
+			LOG_INFO(priv, FW_MSG, "%s %s", "<DBG>",
+				       ((u8 *)msg) + sizeof(msg->hdr)
+					+ sizeof(msg->u.log.log_hdr));
+		else {
+			if (log_msg_format_hex(logbuf, LOG_MSG_SIZE_MAX, buf,
+					le16_to_cpu(msg->hdr.length)
+						+ sizeof(msg->hdr),
+					"<DBG>"))
+				LOG_WARNING(priv, FW_MSG,
+					"TOP DBG message is too long,"
+					"truncating...");
+			LOG_WARNING(priv, FW_MSG, "%s\n", logbuf);
+		}
+		break;
+	default:
+		break;
+	}
+}
+
+static int _log_get_filter_str(u8 *logdefs, int logdefsz, char *buf, int size)
+{
+	int i, pos, len;
+	for (i = 0, pos = 0; (pos < size-1) && (i < logdefsz); i++) {
+		len = snprintf(&buf[pos], size - pos - 1, "0x%02X%02X,",
+				i, logdefs[i]);
+		pos += len;
+	}
+	buf[pos-1] = '\n';
+	buf[pos] = '\0';
+
+	if (i < logdefsz)
+		return -1;
+	return 0;
+}
+
+int log_get_filter_str(char *buf, int size)
+{
+	return _log_get_filter_str(iwmct_logdefs, LOG_SRC_MAX, buf, size);
+}
+
+int log_get_fw_filter_str(char *buf, int size)
+{
+	return _log_get_filter_str(iwmct_fw_logdefs, FW_LOG_SRC_MAX, buf, size);
+}
+
+#define HEXADECIMAL_RADIX	16
+#define LOG_SRC_FORMAT		7 /* log level is in format of "0xXXXX," */
+
+ssize_t show_iwmct_log_level(struct device *d,
+				struct device_attribute *attr, char *buf)
+{
+	struct iwmct_priv *priv = dev_get_drvdata(d);
+	char *str_buf;
+	int buf_size;
+	ssize_t ret;
+
+	buf_size = (LOG_SRC_FORMAT * LOG_SRC_MAX) + 1;
+	str_buf = kzalloc(buf_size, GFP_KERNEL);
+	if (!str_buf) {
+		LOG_ERROR(priv, DEBUGFS,
+			"failed to allocate %d bytes\n", buf_size);
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	if (log_get_filter_str(str_buf, buf_size) < 0) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ret = sprintf(buf, "%s", str_buf);
+
+exit:
+	kfree(str_buf);
+	return ret;
+}
+
+ssize_t store_iwmct_log_level(struct device *d,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct iwmct_priv *priv = dev_get_drvdata(d);
+	char *token, *str_buf = NULL;
+	long val;
+	ssize_t ret = count;
+	u8 src, mask;
+
+	if (!count)
+		goto exit;
+
+	str_buf = kzalloc(count, GFP_KERNEL);
+	if (!str_buf) {
+		LOG_ERROR(priv, DEBUGFS,
+			"failed to allocate %zd bytes\n", count);
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	memcpy(str_buf, buf, count);
+
+	while ((token = strsep(&str_buf, ",")) != NULL) {
+		while (isspace(*token))
+			++token;
+		if (strict_strtol(token, HEXADECIMAL_RADIX, &val)) {
+			LOG_ERROR(priv, DEBUGFS,
+				  "failed to convert string to long %s\n",
+				  token);
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		mask  = val & 0xFF;
+		src = (val & 0XFF00) >> 8;
+		iwmct_log_set_filter(src, mask);
+	}
+
+exit:
+	kfree(str_buf);
+	return ret;
+}
+
+ssize_t show_iwmct_log_level_fw(struct device *d,
+			struct device_attribute *attr, char *buf)
+{
+	struct iwmct_priv *priv = dev_get_drvdata(d);
+	char *str_buf;
+	int buf_size;
+	ssize_t ret;
+
+	buf_size = (LOG_SRC_FORMAT * FW_LOG_SRC_MAX) + 2;
+
+	str_buf = kzalloc(buf_size, GFP_KERNEL);
+	if (!str_buf) {
+		LOG_ERROR(priv, DEBUGFS,
+			"failed to allocate %d bytes\n", buf_size);
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	if (log_get_fw_filter_str(str_buf, buf_size) < 0) {
+		ret = -EINVAL;
+		goto exit;
+	}
+
+	ret = sprintf(buf, "%s", str_buf);
+
+exit:
+	kfree(str_buf);
+	return ret;
+}
+
+ssize_t store_iwmct_log_level_fw(struct device *d,
+			struct device_attribute *attr,
+			const char *buf, size_t count)
+{
+	struct iwmct_priv *priv = dev_get_drvdata(d);
+	struct top_msg cmd;
+	char *token, *str_buf = NULL;
+	ssize_t ret = count;
+	u16 cmdlen = 0;
+	int i;
+	long val;
+	u8 src, mask;
+
+	if (!count)
+		goto exit;
+
+	str_buf = kzalloc(count, GFP_KERNEL);
+	if (!str_buf) {
+		LOG_ERROR(priv, DEBUGFS,
+			"failed to allocate %zd bytes\n", count);
+		ret = -ENOMEM;
+		goto exit;
+	}
+
+	memcpy(str_buf, buf, count);
+
+	cmd.hdr.type = COMM_TYPE_H2D;
+	cmd.hdr.category = COMM_CATEGORY_DEBUG;
+	cmd.hdr.opcode = CMD_DBG_LOG_LEVEL;
+
+	for (i = 0; ((token = strsep(&str_buf, ",")) != NULL) &&
+		     (i < FW_LOG_SRC_MAX); i++) {
+
+		while (isspace(*token))
+			++token;
+
+		if (strict_strtol(token, HEXADECIMAL_RADIX, &val)) {
+			LOG_ERROR(priv, DEBUGFS,
+				  "failed to convert string to long %s\n",
+				  token);
+			ret = -EINVAL;
+			goto exit;
+		}
+
+		mask  = val & 0xFF; /* LSB */
+		src = (val & 0XFF00) >> 8; /* 2nd least significant byte. */
+		iwmct_log_set_fw_filter(src, mask);
+
+		cmd.u.logdefs[i].logsource = src;
+		cmd.u.logdefs[i].sevmask = mask;
+	}
+
+	cmd.hdr.length = cpu_to_le16(i * sizeof(cmd.u.logdefs[0]));
+	cmdlen = (i * sizeof(cmd.u.logdefs[0]) + sizeof(cmd.hdr));
+
+	ret = iwmct_send_hcmd(priv, (u8 *)&cmd, cmdlen);
+	if (ret) {
+		LOG_ERROR(priv, DEBUGFS,
+			  "Failed to send %d bytes of fwcmd, ret=%zd\n",
+			  cmdlen, ret);
+		goto exit;
+	} else
+		LOG_INFO(priv, DEBUGFS, "fwcmd sent (%d bytes)\n", cmdlen);
+
+	ret = count;
+
+exit:
+	kfree(str_buf);
+	return ret;
+}
+
diff --git a/drivers/misc/iwmc3200top/log.h b/drivers/misc/iwmc3200top/log.h
new file mode 100644
index 000000000000..aba8121f978c
--- /dev/null
+++ b/drivers/misc/iwmc3200top/log.h
@@ -0,0 +1,158 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/log.h
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#ifndef __LOG_H__
+#define __LOG_H__
+
+
+/* log severity:
+ * The log levels here match FW log levels
+ * so values need to stay as is */
+#define LOG_SEV_CRITICAL		0
+#define LOG_SEV_ERROR			1
+#define LOG_SEV_WARNING			2
+#define LOG_SEV_INFO			3
+#define LOG_SEV_INFOEX			4
+
+#define LOG_SEV_FILTER_ALL		\
+	(BIT(LOG_SEV_CRITICAL) |	\
+	 BIT(LOG_SEV_ERROR)    |	\
+	 BIT(LOG_SEV_WARNING)  | 	\
+	 BIT(LOG_SEV_INFO)     |	\
+	 BIT(LOG_SEV_INFOEX))
+
+/* log source */
+#define LOG_SRC_INIT			0
+#define LOG_SRC_DEBUGFS			1
+#define LOG_SRC_FW_DOWNLOAD		2
+#define LOG_SRC_FW_MSG			3
+#define LOG_SRC_TST			4
+#define LOG_SRC_IRQ			5
+
+#define	LOG_SRC_MAX			6
+#define	LOG_SRC_ALL			0xFF
+
+/**
+ * Default intitialization runtime log level
+ */
+#ifndef LOG_SEV_FILTER_RUNTIME
+#define LOG_SEV_FILTER_RUNTIME			\
+	(BIT(LOG_SEV_CRITICAL)	|		\
+	 BIT(LOG_SEV_ERROR)	|		\
+	 BIT(LOG_SEV_WARNING))
+#endif
+
+#ifndef FW_LOG_SEV_FILTER_RUNTIME
+#define FW_LOG_SEV_FILTER_RUNTIME	LOG_SEV_FILTER_ALL
+#endif
+
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+/**
+ * Log macros
+ */
+
+#define priv2dev(priv) (&(priv->func)->dev)
+
+#define LOG_CRITICAL(priv, src, fmt, args...)				\
+do {									\
+	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_CRITICAL))	\
+		dev_crit(priv2dev(priv), "%s %d: " fmt,			\
+			__func__, __LINE__, ##args);			\
+} while (0)
+
+#define LOG_ERROR(priv, src, fmt, args...)				\
+do {									\
+	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_ERROR))	\
+		dev_err(priv2dev(priv), "%s %d: " fmt,			\
+			__func__, __LINE__, ##args);			\
+} while (0)
+
+#define LOG_WARNING(priv, src, fmt, args...)				\
+do {									\
+	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_WARNING))	\
+		dev_warn(priv2dev(priv), "%s %d: " fmt,			\
+			 __func__, __LINE__, ##args);			\
+} while (0)
+
+#define LOG_INFO(priv, src, fmt, args...)				\
+do {									\
+	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_INFO))		\
+		dev_info(priv2dev(priv), "%s %d: " fmt,			\
+			 __func__, __LINE__, ##args);			\
+} while (0)
+
+#define LOG_INFOEX(priv, src, fmt, args...)				\
+do {									\
+	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_INFOEX))	\
+		dev_dbg(priv2dev(priv), "%s %d: " fmt,			\
+			 __func__, __LINE__, ##args);			\
+} while (0)
+
+#define LOG_HEXDUMP(src, ptr, len)					\
+do {									\
+	if (iwmct_logdefs[LOG_SRC_ ## src] & BIT(LOG_SEV_INFOEX))	\
+		print_hex_dump(KERN_DEBUG, "", DUMP_PREFIX_NONE,	\
+				16, 1, ptr, len, false);		\
+} while (0)
+
+void iwmct_log_top_message(struct iwmct_priv *priv, u8 *buf, int len);
+
+extern u8 iwmct_logdefs[];
+
+int iwmct_log_set_filter(u8 src, u8 logmask);
+int iwmct_log_set_fw_filter(u8 src, u8 logmask);
+
+ssize_t show_iwmct_log_level(struct device *d,
+			struct device_attribute *attr, char *buf);
+ssize_t store_iwmct_log_level(struct device *d,
+			struct device_attribute *attr,
+			const char *buf, size_t count);
+ssize_t show_iwmct_log_level_fw(struct device *d,
+			struct device_attribute *attr, char *buf);
+ssize_t store_iwmct_log_level_fw(struct device *d,
+			struct device_attribute *attr,
+			const char *buf, size_t count);
+
+#else
+
+#define LOG_CRITICAL(priv, src, fmt, args...)
+#define LOG_ERROR(priv, src, fmt, args...)
+#define LOG_WARNING(priv, src, fmt, args...)
+#define LOG_INFO(priv, src, fmt, args...)
+#define LOG_INFOEX(priv, src, fmt, args...)
+#define LOG_HEXDUMP(src, ptr, len)
+
+static inline void iwmct_log_top_message(struct iwmct_priv *priv,
+					 u8 *buf, int len) {}
+static inline int iwmct_log_set_filter(u8 src, u8 logmask) { return 0; }
+static inline int iwmct_log_set_fw_filter(u8 src, u8 logmask) { return 0; }
+
+#endif /* CONFIG_IWMC3200TOP_DEBUG */
+
+int log_get_filter_str(char *buf, int size);
+int log_get_fw_filter_str(char *buf, int size);
+
+#endif /* __LOG_H__ */
diff --git a/drivers/misc/iwmc3200top/main.c b/drivers/misc/iwmc3200top/main.c
new file mode 100644
index 000000000000..fafcaa481d74
--- /dev/null
+++ b/drivers/misc/iwmc3200top/main.c
@@ -0,0 +1,678 @@
+/*
+ * iwmc3200top - Intel Wireless MultiCom 3200 Top Driver
+ * drivers/misc/iwmc3200top/main.c
+ *
+ * Copyright (C) 2009 Intel Corporation. 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.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You 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.
+ *
+ *
+ * Author Name: Maxim Grabarnik <maxim.grabarnink@intel.com>
+ *  -
+ *
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/debugfs.h>
+#include <linux/mmc/sdio_ids.h>
+#include <linux/mmc/sdio_func.h>
+#include <linux/mmc/sdio.h>
+
+#include "iwmc3200top.h"
+#include "log.h"
+#include "fw-msg.h"
+#include "debugfs.h"
+
+
+#define DRIVER_DESCRIPTION "Intel(R) IWMC 3200 Top Driver"
+#define DRIVER_COPYRIGHT "Copyright (c) 2008 Intel Corporation."
+
+#define DRIVER_VERSION  "0.1.62"
+
+MODULE_DESCRIPTION(DRIVER_DESCRIPTION);
+MODULE_VERSION(DRIVER_VERSION);
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR(DRIVER_COPYRIGHT);
+MODULE_FIRMWARE(FW_NAME(FW_API_VER));
+
+/*
+ * This workers main task is to wait for OP_OPR_ALIVE
+ * from TOP FW until ALIVE_MSG_TIMOUT timeout is elapsed.
+ * When OP_OPR_ALIVE received it will issue
+ * a call to "bus_rescan_devices".
+ */
+static void iwmct_rescan_worker(struct work_struct *ws)
+{
+	struct iwmct_priv *priv;
+	int ret;
+
+	priv = container_of(ws, struct iwmct_priv, bus_rescan_worker);
+
+	LOG_INFO(priv, FW_MSG, "Calling bus_rescan\n");
+
+	ret = bus_rescan_devices(priv->func->dev.bus);
+	if (ret < 0)
+		LOG_INFO(priv, FW_DOWNLOAD, "bus_rescan_devices FAILED!!!\n");
+}
+
+static void op_top_message(struct iwmct_priv *priv, struct top_msg *msg)
+{
+	switch (msg->hdr.opcode) {
+	case OP_OPR_ALIVE:
+		LOG_INFO(priv, FW_MSG, "Got ALIVE from device, wake rescan\n");
+		queue_work(priv->bus_rescan_wq, &priv->bus_rescan_worker);
+		break;
+	default:
+		LOG_INFO(priv, FW_MSG, "Received msg opcode 0x%X\n",
+			msg->hdr.opcode);
+		break;
+	}
+}
+
+
+static void handle_top_message(struct iwmct_priv *priv, u8 *buf, int len)
+{
+	struct top_msg *msg;
+
+	msg = (struct top_msg *)buf;
+
+	if (msg->hdr.type != COMM_TYPE_D2H) {
+		LOG_ERROR(priv, FW_MSG,
+			"Message from TOP with invalid message type 0x%X\n",
+			msg->hdr.type);
+		return;
+	}
+
+	if (len < sizeof(msg->hdr)) {
+		LOG_ERROR(priv, FW_MSG,
+			"Message from TOP is too short for message header "
+			"received %d bytes, expected at least %zd bytes\n",
+			len, sizeof(msg->hdr));
+		return;
+	}
+
+	if (len < le16_to_cpu(msg->hdr.length) + sizeof(msg->hdr)) {
+		LOG_ERROR(priv, FW_MSG,
+			"Message length (%d bytes) is shorter than "
+			"in header (%d bytes)\n",
+			len, le16_to_cpu(msg->hdr.length));
+		return;
+	}
+
+	switch (msg->hdr.category) {
+	case COMM_CATEGORY_OPERATIONAL:
+		op_top_message(priv, (struct top_msg *)buf);
+		break;
+
+	case COMM_CATEGORY_DEBUG:
+	case COMM_CATEGORY_TESTABILITY:
+	case COMM_CATEGORY_DIAGNOSTICS:
+		iwmct_log_top_message(priv, buf, len);
+		break;
+
+	default:
+		LOG_ERROR(priv, FW_MSG,
+			"Message from TOP with unknown category 0x%X\n",
+			msg->hdr.category);
+		break;
+	}
+}
+
+int iwmct_send_hcmd(struct iwmct_priv *priv, u8 *cmd, u16 len)
+{
+	int ret;
+	u8 *buf;
+
+	LOG_INFOEX(priv, FW_MSG, "Sending hcmd:\n");
+
+	/* add padding to 256 for IWMC */
+	((struct top_msg *)cmd)->hdr.flags |= CMD_FLAG_PADDING_256;
+
+	LOG_HEXDUMP(FW_MSG, cmd, len);
+
+	if (len > FW_HCMD_BLOCK_SIZE) {
+		LOG_ERROR(priv, FW_MSG, "size %d exceeded hcmd max size %d\n",
+			  len, FW_HCMD_BLOCK_SIZE);
+		return -1;
+	}
+
+	buf = kzalloc(FW_HCMD_BLOCK_SIZE, GFP_KERNEL);
+	if (!buf) {
+		LOG_ERROR(priv, FW_MSG, "kzalloc error, buf size %d\n",
+			  FW_HCMD_BLOCK_SIZE);
+		return -1;
+	}
+
+	memcpy(buf, cmd, len);
+
+	sdio_claim_host(priv->func);
+	ret = sdio_memcpy_toio(priv->func, IWMC_SDIO_DATA_ADDR, buf,
+			       FW_HCMD_BLOCK_SIZE);
+	sdio_release_host(priv->func);
+
+	kfree(buf);
+	return ret;
+}
+
+int iwmct_tx(struct iwmct_priv *priv, unsigned int addr,
+	void *src, int count)
+{
+	int ret;
+
+	sdio_claim_host(priv->func);
+	ret = sdio_memcpy_toio(priv->func, addr, src, count);
+	sdio_release_host(priv->func);
+
+	return ret;
+}
+
+static void iwmct_irq_read_worker(struct work_struct *ws)
+{
+	struct iwmct_priv *priv;
+	struct iwmct_work_struct *read_req;
+	__le32 *buf = NULL;
+	int ret;
+	int iosize;
+	u32 barker;
+	bool is_barker;
+
+	priv = container_of(ws, struct iwmct_priv, isr_worker);
+
+	LOG_INFO(priv, IRQ, "enter iwmct_irq_read_worker %p\n", ws);
+
+	/* --------------------- Handshake with device -------------------- */
+	sdio_claim_host(priv->func);
+
+	/* all list manipulations have to be protected by
+	 * sdio_claim_host/sdio_release_host */
+	if (list_empty(&priv->read_req_list)) {
+		LOG_ERROR(priv, IRQ, "read_req_list empty in read worker\n");
+		goto exit_release;
+	}
+
+	read_req = list_entry(priv->read_req_list.next,
+			      struct iwmct_work_struct, list);
+
+	list_del(&read_req->list);
+	iosize = read_req->iosize;
+	kfree(read_req);
+
+	buf = kzalloc(iosize, GFP_KERNEL);
+	if (!buf) {
+		LOG_ERROR(priv, IRQ, "kzalloc error, buf size %d\n", iosize);
+		goto exit_release;
+	}
+
+	LOG_INFO(priv, IRQ, "iosize=%d, buf=%p, func=%d\n",
+				iosize, buf, priv->func->num);
+
+	/* read from device */
+	ret = sdio_memcpy_fromio(priv->func, buf, IWMC_SDIO_DATA_ADDR, iosize);
+	if (ret) {
+		LOG_ERROR(priv, IRQ, "error %d reading buffer\n", ret);
+		goto exit_release;
+	}
+
+	LOG_HEXDUMP(IRQ, (u8 *)buf, iosize);
+
+	barker = le32_to_cpu(buf[0]);
+
+	/* Verify whether it's a barker and if not - treat as regular Rx */
+	if (barker == IWMC_BARKER_ACK ||
+	    (barker & BARKER_DNLOAD_BARKER_MSK) == IWMC_BARKER_REBOOT) {
+
+		/* Valid Barker is equal on first 4 dwords */
+		is_barker = (buf[1] == buf[0]) &&
+			    (buf[2] == buf[0]) &&
+			    (buf[3] == buf[0]);
+
+		if (!is_barker) {
+			LOG_WARNING(priv, IRQ,
+				"Potentially inconsistent barker "
+				"%08X_%08X_%08X_%08X\n",
+				le32_to_cpu(buf[0]), le32_to_cpu(buf[1]),
+				le32_to_cpu(buf[2]), le32_to_cpu(buf[3]));
+		}
+	} else {
+		is_barker = false;
+	}
+
+	/* Handle Top CommHub message */
+	if (!is_barker) {
+		sdio_release_host(priv->func);
+		handle_top_message(priv, (u8 *)buf, iosize);
+		goto exit;
+	} else if (barker == IWMC_BARKER_ACK) { /* Handle barkers */
+		if (atomic_read(&priv->dev_sync) == 0) {
+			LOG_ERROR(priv, IRQ,
+				  "ACK barker arrived out-of-sync\n");
+			goto exit_release;
+		}
+
+		/* Continuing to FW download (after Sync is completed)*/
+		atomic_set(&priv->dev_sync, 0);
+		LOG_INFO(priv, IRQ, "ACK barker arrived "
+				"- starting FW download\n");
+	} else { /* REBOOT barker */
+		LOG_INFO(priv, IRQ, "Recieved reboot barker: %x\n", barker);
+		priv->barker = barker;
+
+		if (barker & BARKER_DNLOAD_SYNC_MSK) {
+			/* Send the same barker back */
+			ret = sdio_memcpy_toio(priv->func, IWMC_SDIO_DATA_ADDR,
+					       buf, iosize);
+			if (ret) {
+				LOG_ERROR(priv, IRQ,
+					 "error %d echoing barker\n", ret);
+				goto exit_release;
+			}
+			LOG_INFO(priv, IRQ, "Echoing barker to device\n");
+			atomic_set(&priv->dev_sync, 1);
+			goto exit_release;
+		}
+
+		/* Continuing to FW download (without Sync) */
+		LOG_INFO(priv, IRQ, "No sync requested "
+				    "- starting FW download\n");
+	}
+
+	sdio_release_host(priv->func);
+
+
+	LOG_INFO(priv, IRQ, "barker download request 0x%x is:\n", priv->barker);
+	LOG_INFO(priv, IRQ, "*******  Top FW %s requested ********\n",
+			(priv->barker & BARKER_DNLOAD_TOP_MSK) ? "was" : "not");
+	LOG_INFO(priv, IRQ, "*******  GPS FW %s requested ********\n",
+			(priv->barker & BARKER_DNLOAD_GPS_MSK) ? "was" : "not");
+	LOG_INFO(priv, IRQ, "*******  BT FW %s requested ********\n",
+			(priv->barker & BARKER_DNLOAD_BT_MSK) ? "was" : "not");
+
+	if (priv->dbg.fw_download)
+		iwmct_fw_load(priv);
+	else
+		LOG_ERROR(priv, IRQ, "FW download not allowed\n");
+
+	goto exit;
+
+exit_release:
+	sdio_release_host(priv->func);
+exit:
+	kfree(buf);
+	LOG_INFO(priv, IRQ, "exit iwmct_irq_read_worker\n");
+}
+
+static void iwmct_irq(struct sdio_func *func)
+{
+	struct iwmct_priv *priv;
+	int val, ret;
+	int iosize;
+	int addr = IWMC_SDIO_INTR_GET_SIZE_ADDR;
+	struct iwmct_work_struct *read_req;
+
+	priv = sdio_get_drvdata(func);
+
+	LOG_INFO(priv, IRQ, "enter iwmct_irq\n");
+
+	/* read the function's status register */
+	val = sdio_readb(func, IWMC_SDIO_INTR_STATUS_ADDR, &ret);
+
+	LOG_INFO(priv, IRQ, "iir value = %d, ret=%d\n", val, ret);
+
+	if (!val) {
+		LOG_ERROR(priv, IRQ, "iir = 0, exiting ISR\n");
+		goto exit_clear_intr;
+	}
+
+
+	/*
+	 * read 2 bytes of the transaction size
+	 * IMPORTANT: sdio transaction size has to be read before clearing
+	 * sdio interrupt!!!
+	 */
+	val = sdio_readb(priv->func, addr++, &ret);
+	iosize = val;
+	val = sdio_readb(priv->func, addr++, &ret);
+	iosize += val << 8;
+
+	LOG_INFO(priv, IRQ, "READ size %d\n", iosize);
+
+	if (iosize == 0) {
+		LOG_ERROR(priv, IRQ, "READ size %d, exiting ISR\n", iosize);
+		goto exit_clear_intr;
+	}
+
+	/* allocate a work structure to pass iosize to the worker */
+	read_req = kzalloc(sizeof(struct iwmct_work_struct), GFP_KERNEL);
+	if (!read_req) {
+		LOG_ERROR(priv, IRQ, "failed to allocate read_req, exit ISR\n");
+		goto exit_clear_intr;
+	}
+
+	INIT_LIST_HEAD(&read_req->list);
+	read_req->iosize = iosize;
+
+	list_add_tail(&priv->read_req_list, &read_req->list);
+
+	/* clear the function's interrupt request bit (write 1 to clear) */
+	sdio_writeb(func, 1, IWMC_SDIO_INTR_CLEAR_ADDR, &ret);
+
+	queue_work(priv->wq, &priv->isr_worker);
+
+	LOG_INFO(priv, IRQ, "exit iwmct_irq\n");
+
+	return;
+
+exit_clear_intr:
+	/* clear the function's interrupt request bit (write 1 to clear) */
+	sdio_writeb(func, 1, IWMC_SDIO_INTR_CLEAR_ADDR, &ret);
+}
+
+
+static int blocks;
+module_param(blocks, int, 0604);
+MODULE_PARM_DESC(blocks, "max_blocks_to_send");
+
+static int dump;
+module_param(dump, bool, 0604);
+MODULE_PARM_DESC(dump, "dump_hex_content");
+
+static int jump = 1;
+module_param(jump, bool, 0604);
+
+static int direct = 1;
+module_param(direct, bool, 0604);
+
+static int checksum = 1;
+module_param(checksum, bool, 0604);
+
+static int fw_download = 1;
+module_param(fw_download, bool, 0604);
+
+static int block_size = IWMC_SDIO_BLK_SIZE;
+module_param(block_size, int, 0404);
+
+static int download_trans_blks = IWMC_DEFAULT_TR_BLK;
+module_param(download_trans_blks, int, 0604);
+
+static int rubbish_barker;
+module_param(rubbish_barker, bool, 0604);
+
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+static int log_level[LOG_SRC_MAX];
+static unsigned int log_level_argc;
+module_param_array(log_level, int, &log_level_argc, 0604);
+MODULE_PARM_DESC(log_level, "log_level");
+
+static int log_level_fw[FW_LOG_SRC_MAX];
+static unsigned int log_level_fw_argc;
+module_param_array(log_level_fw, int, &log_level_fw_argc, 0604);
+MODULE_PARM_DESC(log_level_fw, "log_level_fw");
+#endif
+
+void iwmct_dbg_init_params(struct iwmct_priv *priv)
+{
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+	int i;
+
+	for (i = 0; i < log_level_argc; i++) {
+		dev_notice(&priv->func->dev, "log_level[%d]=0x%X\n",
+						i, log_level[i]);
+		iwmct_log_set_filter((log_level[i] >> 8) & 0xFF,
+			       log_level[i] & 0xFF);
+	}
+	for (i = 0; i < log_level_fw_argc; i++) {
+		dev_notice(&priv->func->dev, "log_level_fw[%d]=0x%X\n",
+						i, log_level_fw[i]);
+		iwmct_log_set_fw_filter((log_level_fw[i] >> 8) & 0xFF,
+				  log_level_fw[i] & 0xFF);
+	}
+#endif
+
+	priv->dbg.blocks = blocks;
+	LOG_INFO(priv, INIT, "blocks=%d\n", blocks);
+	priv->dbg.dump = (bool)dump;
+	LOG_INFO(priv, INIT, "dump=%d\n", dump);
+	priv->dbg.jump = (bool)jump;
+	LOG_INFO(priv, INIT, "jump=%d\n", jump);
+	priv->dbg.direct = (bool)direct;
+	LOG_INFO(priv, INIT, "direct=%d\n", direct);
+	priv->dbg.checksum = (bool)checksum;
+	LOG_INFO(priv, INIT, "checksum=%d\n", checksum);
+	priv->dbg.fw_download = (bool)fw_download;
+	LOG_INFO(priv, INIT, "fw_download=%d\n", fw_download);
+	priv->dbg.block_size = block_size;
+	LOG_INFO(priv, INIT, "block_size=%d\n", block_size);
+	priv->dbg.download_trans_blks = download_trans_blks;
+	LOG_INFO(priv, INIT, "download_trans_blks=%d\n", download_trans_blks);
+}
+
+/*****************************************************************************
+ *
+ * sysfs attributes
+ *
+ *****************************************************************************/
+static ssize_t show_iwmct_fw_version(struct device *d,
+				  struct device_attribute *attr, char *buf)
+{
+	struct iwmct_priv *priv = dev_get_drvdata(d);
+	return sprintf(buf, "%s\n", priv->dbg.label_fw);
+}
+static DEVICE_ATTR(cc_label_fw, S_IRUGO, show_iwmct_fw_version, NULL);
+
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+static DEVICE_ATTR(log_level, S_IWUSR | S_IRUGO,
+		   show_iwmct_log_level, store_iwmct_log_level);
+static DEVICE_ATTR(log_level_fw, S_IWUSR | S_IRUGO,
+		   show_iwmct_log_level_fw, store_iwmct_log_level_fw);
+#endif
+
+static struct attribute *iwmct_sysfs_entries[] = {
+	&dev_attr_cc_label_fw.attr,
+#ifdef CONFIG_IWMC3200TOP_DEBUG
+	&dev_attr_log_level.attr,
+	&dev_attr_log_level_fw.attr,
+#endif
+	NULL
+};
+
+static struct attribute_group iwmct_attribute_group = {
+	.name = NULL,		/* put in device directory */
+	.attrs = iwmct_sysfs_entries,
+};
+
+
+static int iwmct_probe(struct sdio_func *func,
+			   const struct sdio_device_id *id)
+{
+	struct iwmct_priv *priv;
+	int ret;
+	int val = 1;
+	int addr = IWMC_SDIO_INTR_ENABLE_ADDR;
+
+	dev_dbg(&func->dev, "enter iwmct_probe\n");
+
+	dev_dbg(&func->dev, "IRQ polling period id %u msecs, HZ is %d\n",
+		jiffies_to_msecs(2147483647), HZ);
+
+	priv = kzalloc(sizeof(struct iwmct_priv), GFP_KERNEL);
+	if (!priv) {
+		dev_err(&func->dev, "kzalloc error\n");
+		return -ENOMEM;
+	}
+	priv->func = func;
+	sdio_set_drvdata(func, priv);
+
+
+	/* create drivers work queue */
+	priv->wq = create_workqueue(DRV_NAME "_wq");
+	priv->bus_rescan_wq = create_workqueue(DRV_NAME "_rescan_wq");
+	INIT_WORK(&priv->bus_rescan_worker, iwmct_rescan_worker);
+	INIT_WORK(&priv->isr_worker, iwmct_irq_read_worker);
+
+	init_waitqueue_head(&priv->wait_q);
+
+	sdio_claim_host(func);
+	/* FIXME: Remove after it is fixed in the Boot ROM upgrade */
+	func->enable_timeout = 10;
+
+	/* In our HW, setting the block size also wakes up the boot rom. */
+	ret = sdio_set_block_size(func, priv->dbg.block_size);
+	if (ret) {
+		LOG_ERROR(priv, INIT,
+			"sdio_set_block_size() failure: %d\n", ret);
+		goto error_sdio_enable;
+	}
+
+	ret = sdio_enable_func(func);
+	if (ret) {
+		LOG_ERROR(priv, INIT, "sdio_enable_func() failure: %d\n", ret);
+		goto error_sdio_enable;
+	}
+
+	/* init reset and dev_sync states */
+	atomic_set(&priv->reset, 0);
+	atomic_set(&priv->dev_sync, 0);
+
+	/* init read req queue */
+	INIT_LIST_HEAD(&priv->read_req_list);
+
+	/* process configurable parameters */
+	iwmct_dbg_init_params(priv);
+	ret = sysfs_create_group(&func->dev.kobj, &iwmct_attribute_group);
+	if (ret) {
+		LOG_ERROR(priv, INIT, "Failed to register attributes and "
+			 "initialize module_params\n");
+		goto error_dev_attrs;
+	}
+
+	iwmct_dbgfs_register(priv, DRV_NAME);
+
+	if (!priv->dbg.direct && priv->dbg.download_trans_blks > 8) {
+		LOG_INFO(priv, INIT,
+			 "Reducing transaction to 8 blocks = 2K (from %d)\n",
+			 priv->dbg.download_trans_blks);
+		priv->dbg.download_trans_blks = 8;
+	}
+	priv->trans_len = priv->dbg.download_trans_blks * priv->dbg.block_size;
+	LOG_INFO(priv, INIT, "Transaction length = %d\n", priv->trans_len);
+
+	ret = sdio_claim_irq(func, iwmct_irq);
+	if (ret) {
+		LOG_ERROR(priv, INIT, "sdio_claim_irq() failure: %d\n", ret);
+		goto error_claim_irq;
+	}
+
+
+	/* Enable function's interrupt */
+	sdio_writeb(priv->func, val, addr, &ret);
+	if (ret) {
+		LOG_ERROR(priv, INIT, "Failure writing to "
+			  "Interrupt Enable Register (%d): %d\n", addr, ret);
+		goto error_enable_int;
+	}
+
+	sdio_release_host(func);
+
+	LOG_INFO(priv, INIT, "exit iwmct_probe\n");
+
+	return ret;
+
+error_enable_int:
+	sdio_release_irq(func);
+error_claim_irq:
+	sdio_disable_func(func);
+error_dev_attrs:
+	iwmct_dbgfs_unregister(priv->dbgfs);
+	sysfs_remove_group(&func->dev.kobj, &iwmct_attribute_group);
+error_sdio_enable:
+	sdio_release_host(func);
+	return ret;
+}
+
+static void iwmct_remove(struct sdio_func *func)
+{
+	struct iwmct_work_struct *read_req;
+	struct iwmct_priv *priv = sdio_get_drvdata(func);
+
+	priv = sdio_get_drvdata(func);
+
+	LOG_INFO(priv, INIT, "enter\n");
+
+	sdio_claim_host(func);
+	sdio_release_irq(func);
+	sdio_release_host(func);
+
+	/* Safely destroy osc workqueue */
+	destroy_workqueue(priv->bus_rescan_wq);
+	destroy_workqueue(priv->wq);
+
+	sdio_claim_host(func);
+	sdio_disable_func(func);
+	sysfs_remove_group(&func->dev.kobj, &iwmct_attribute_group);
+	iwmct_dbgfs_unregister(priv->dbgfs);
+	sdio_release_host(func);
+
+	/* free read requests */
+	while (!list_empty(&priv->read_req_list)) {
+		read_req = list_entry(priv->read_req_list.next,
+			struct iwmct_work_struct, list);
+
+		list_del(&read_req->list);
+		kfree(read_req);
+	}
+
+	kfree(priv);
+}
+
+
+static const struct sdio_device_id iwmct_ids[] = {
+	/* Intel Wireless MultiCom 3200 Top Driver */
+	{ SDIO_DEVICE(SDIO_VENDOR_ID_INTEL, 0x1404)},
+	{ },	/* Terminating entry */
+};
+
+MODULE_DEVICE_TABLE(sdio, iwmct_ids);
+
+static struct sdio_driver iwmct_driver = {
+	.probe		= iwmct_probe,
+	.remove		= iwmct_remove,
+	.name		= DRV_NAME,
+	.id_table	= iwmct_ids,
+};
+
+static int __init iwmct_init(void)
+{
+	int rc;
+
+	/* Default log filter settings */
+	iwmct_log_set_filter(LOG_SRC_ALL, LOG_SEV_FILTER_RUNTIME);
+	iwmct_log_set_filter(LOG_SRC_FW_MSG, LOG_SEV_FILTER_ALL);
+	iwmct_log_set_fw_filter(LOG_SRC_ALL, FW_LOG_SEV_FILTER_RUNTIME);
+
+	rc = sdio_register_driver(&iwmct_driver);
+
+	return rc;
+}
+
+static void __exit iwmct_exit(void)
+{
+	sdio_unregister_driver(&iwmct_driver);
+}
+
+module_init(iwmct_init);
+module_exit(iwmct_exit);
+
diff --git a/drivers/net/3c501.c b/drivers/net/3c501.c
index f60309175ef5..4d4cad393dce 100644
--- a/drivers/net/3c501.c
+++ b/drivers/net/3c501.c
@@ -249,11 +249,11 @@ static int __init el1_probe1(struct net_device *dev, int ioaddr)
 	 *	for the Sager NP943 prefix.
 	 */
 
-	if (station_addr[0] == 0x02  &&  station_addr[1] == 0x60
-						&& station_addr[2] == 0x8c)
+	if (station_addr[0] == 0x02 && station_addr[1] == 0x60 &&
+	    station_addr[2] == 0x8c)
 		mname = "3c501";
-	else if (station_addr[0] == 0x00  &&  station_addr[1] == 0x80
-						&& station_addr[2] == 0xC8)
+	else if (station_addr[0] == 0x00 && station_addr[1] == 0x80 &&
+		 station_addr[2] == 0xC8)
 		mname = "NP943";
 	else {
 		release_region(ioaddr, EL1_IO_EXTENT);
@@ -345,7 +345,7 @@ static int el_open(struct net_device *dev)
 	if (el_debug > 2)
 		pr_debug("%s: Doing el_open()...\n", dev->name);
 
-	retval = request_irq(dev->irq, &el_interrupt, 0, dev->name, dev);
+	retval = request_irq(dev->irq, el_interrupt, 0, dev->name, dev);
 	if (retval)
 		return retval;
 
diff --git a/drivers/net/3c503.c b/drivers/net/3c503.c
index c71e12d05f6e..66e0323c1839 100644
--- a/drivers/net/3c503.c
+++ b/drivers/net/3c503.c
@@ -214,8 +214,8 @@ el2_probe1(struct net_device *dev, int ioaddr)
     iobase_reg = inb(ioaddr+0x403);
     membase_reg = inb(ioaddr+0x404);
     /* ASIC location registers should be 0 or have only a single bit set. */
-    if (   (iobase_reg  & (iobase_reg - 1))
-	|| (membase_reg & (membase_reg - 1))) {
+    if ((iobase_reg  & (iobase_reg - 1)) ||
+	(membase_reg & (membase_reg - 1))) {
 	retval = -ENODEV;
 	goto out1;
     }
@@ -291,8 +291,8 @@ el2_probe1(struct net_device *dev, int ioaddr)
 	    writel(0xba5eba5e, mem_base);
 	    for (i = sizeof(test_val); i < EL2_MEMSIZE; i+=sizeof(test_val)) {
 		writel(test_val, mem_base + i);
-		if (readl(mem_base) != 0xba5eba5e
-		    || readl(mem_base + i) != test_val) {
+		if (readl(mem_base) != 0xba5eba5e ||
+		    readl(mem_base + i) != test_val) {
 		    pr_warning("3c503: memory failure or memory address conflict.\n");
 		    dev->mem_start = 0;
 		    ei_status.name = "3c503-PIO";
@@ -397,9 +397,10 @@ el2_open(struct net_device *dev)
 		unsigned long cookie = probe_irq_on();
 		outb_p(0x04 << ((*irqp == 9) ? 2 : *irqp), E33G_IDCFR);
 		outb_p(0x00, E33G_IDCFR);
-		if (*irqp == probe_irq_off(cookie)	/* It's a good IRQ line! */
-		    && ((retval = request_irq(dev->irq = *irqp,
-		    eip_interrupt, 0, dev->name, dev)) == 0))
+		if (*irqp == probe_irq_off(cookie) &&	/* It's a good IRQ line! */
+		    ((retval = request_irq(dev->irq = *irqp,
+					   eip_interrupt, 0,
+					   dev->name, dev)) == 0))
 		    break;
 	    } else {
 		    if (retval != -EBUSY)
diff --git a/drivers/net/3c505.c b/drivers/net/3c505.c
index a21c9d15ef8a..9257d7ce0378 100644
--- a/drivers/net/3c505.c
+++ b/drivers/net/3c505.c
@@ -886,7 +886,7 @@ static int elp_open(struct net_device *dev)
 	/*
 	 * install our interrupt service routine
 	 */
-	if ((retval = request_irq(dev->irq, &elp_interrupt, 0, dev->name, dev))) {
+	if ((retval = request_irq(dev->irq, elp_interrupt, 0, dev->name, dev))) {
 		pr_err("%s: could not allocate IRQ%d\n", dev->name, dev->irq);
 		return retval;
 	}
diff --git a/drivers/net/3c507.c b/drivers/net/3c507.c
index a6dc8bcbc7df..fbc231153e55 100644
--- a/drivers/net/3c507.c
+++ b/drivers/net/3c507.c
@@ -399,7 +399,7 @@ static int __init el16_probe1(struct net_device *dev, int ioaddr)
 
 	irq = inb(ioaddr + IRQ_CONFIG) & 0x0f;
 
-	irqval = request_irq(irq, &el16_interrupt, 0, DRV_NAME, dev);
+	irqval = request_irq(irq, el16_interrupt, 0, DRV_NAME, dev);
 	if (irqval) {
 		pr_cont("\n");
 		pr_err("3c507: unable to get IRQ %d (irqval=%d).\n", irq, irqval);
@@ -836,8 +836,8 @@ static void el16_rx(struct net_device *dev)
 		void __iomem *data_frame = lp->base + data_buffer_addr;
 		ushort pkt_len = readw(data_frame);
 
-		if (rfd_cmd != 0 || data_buffer_addr != rx_head + 22
-			|| (pkt_len & 0xC000) != 0xC000) {
+		if (rfd_cmd != 0 || data_buffer_addr != rx_head + 22 ||
+		    (pkt_len & 0xC000) != 0xC000) {
 			pr_err("%s: Rx frame at %#x corrupted, "
 			       "status %04x cmd %04x next %04x "
 			       "data-buf @%04x %04x.\n",
diff --git a/drivers/net/3c509.c b/drivers/net/3c509.c
index 3b00a4e927aa..9d85efce5916 100644
--- a/drivers/net/3c509.c
+++ b/drivers/net/3c509.c
@@ -253,9 +253,9 @@ static int el3_isa_id_sequence(__be16 *phys_addr)
 		   This check is needed in order not to register them twice. */
 		for (i = 0; i < el3_cards; i++) {
 			struct el3_private *lp = netdev_priv(el3_devs[i]);
-			if (lp->type == EL3_PNP
-			    && !memcmp(phys_addr, el3_devs[i]->dev_addr,
-				       ETH_ALEN)) {
+			if (lp->type == EL3_PNP &&
+			    !memcmp(phys_addr, el3_devs[i]->dev_addr,
+				    ETH_ALEN)) {
 				if (el3_debug > 3)
 					pr_debug("3c509 with address %02x %02x %02x %02x %02x %02x was found by ISAPnP\n",
 						phys_addr[0] & 0xff, phys_addr[0] >> 8,
@@ -780,7 +780,7 @@ el3_open(struct net_device *dev)
 	outw(RxReset, ioaddr + EL3_CMD);
 	outw(SetStatusEnb | 0x00, ioaddr + EL3_CMD);
 
-	i = request_irq(dev->irq, &el3_interrupt, 0, dev->name, dev);
+	i = request_irq(dev->irq, el3_interrupt, 0, dev->name, dev);
 	if (i)
 		return i;
 
@@ -835,8 +835,8 @@ el3_start_xmit(struct sk_buff *skb, struct net_device *dev)
 #ifndef final_version
 	{	/* Error-checking code, delete someday. */
 		ushort status = inw(ioaddr + EL3_STATUS);
-		if (status & 0x0001 		/* IRQ line active, missed one. */
-			&& inw(ioaddr + EL3_STATUS) & 1) { 			/* Make sure. */
+		if (status & 0x0001 && 		/* IRQ line active, missed one. */
+		    inw(ioaddr + EL3_STATUS) & 1) { 			/* Make sure. */
 			pr_debug("%s: Missed interrupt, status then %04x now %04x"
 				   "  Tx %2.2x Rx %4.4x.\n", dev->name, status,
 				   inw(ioaddr + EL3_STATUS), inb(ioaddr + TX_STATUS),
diff --git a/drivers/net/3c515.c b/drivers/net/3c515.c
index 4adcb950f5f1..063b049ffe55 100644
--- a/drivers/net/3c515.c
+++ b/drivers/net/3c515.c
@@ -764,13 +764,14 @@ static int corkscrew_open(struct net_device *dev)
 	/* Use the now-standard shared IRQ implementation. */
 	if (vp->capabilities == 0x11c7) {
 		/* Corkscrew: Cannot share ISA resources. */
-		if (dev->irq == 0
-		    || dev->dma == 0
-		    || request_irq(dev->irq, &corkscrew_interrupt, 0,
-				   vp->product_name, dev)) return -EAGAIN;
+		if (dev->irq == 0 ||
+		    dev->dma == 0 ||
+		    request_irq(dev->irq, corkscrew_interrupt, 0,
+				vp->product_name, dev))
+			return -EAGAIN;
 		enable_dma(dev->dma);
 		set_dma_mode(dev->dma, DMA_MODE_CASCADE);
-	} else if (request_irq(dev->irq, &corkscrew_interrupt, IRQF_SHARED,
+	} else if (request_irq(dev->irq, corkscrew_interrupt, IRQF_SHARED,
 			       vp->product_name, dev)) {
 		return -EAGAIN;
 	}
@@ -1368,8 +1369,8 @@ static int boomerang_rx(struct net_device *dev)
 
 			/* Check if the packet is long enough to just accept without
 			   copying to a properly sized skbuff. */
-			if (pkt_len < rx_copybreak
-			    && (skb = dev_alloc_skb(pkt_len + 4)) != NULL) {
+			if (pkt_len < rx_copybreak &&
+			    (skb = dev_alloc_skb(pkt_len + 4)) != NULL) {
 				skb_reserve(skb, 2);	/* Align IP on 16 byte boundaries */
 				/* 'skb_put()' points to the start of sk_buff data area. */
 				memcpy(skb_put(skb, pkt_len),
diff --git a/drivers/net/3c523.c b/drivers/net/3c523.c
index cb0b730799ba..27d80ca5e4c0 100644
--- a/drivers/net/3c523.c
+++ b/drivers/net/3c523.c
@@ -288,7 +288,7 @@ static int elmc_open(struct net_device *dev)
 
 	elmc_id_attn586();	/* disable interrupts */
 
-	ret = request_irq(dev->irq, &elmc_interrupt, IRQF_SHARED | IRQF_SAMPLE_RANDOM,
+	ret = request_irq(dev->irq, elmc_interrupt, IRQF_SHARED | IRQF_SAMPLE_RANDOM,
 			  dev->name, dev);
 	if (ret) {
 		pr_err("%s: couldn't get irq %d\n", dev->name, dev->irq);
diff --git a/drivers/net/3c527.c b/drivers/net/3c527.c
index 6021e6dded8f..36c4191e7bca 100644
--- a/drivers/net/3c527.c
+++ b/drivers/net/3c527.c
@@ -443,7 +443,7 @@ static int __init mc32_probe1(struct net_device *dev, int slot)
 	 *	Grab the IRQ
 	 */
 
-	err = request_irq(dev->irq, &mc32_interrupt, IRQF_SHARED | IRQF_SAMPLE_RANDOM, DRV_NAME, dev);
+	err = request_irq(dev->irq, mc32_interrupt, IRQF_SHARED | IRQF_SAMPLE_RANDOM, DRV_NAME, dev);
 	if (err) {
 		release_region(dev->base_addr, MC32_IO_EXTENT);
 		pr_err("%s: unable to get IRQ %d.\n", DRV_NAME, dev->irq);
@@ -1168,8 +1168,8 @@ static void mc32_rx_ring(struct net_device *dev)
 
 			/* Try to save time by avoiding a copy on big frames */
 
-			if ((length > RX_COPYBREAK)
-			    && ((newskb=dev_alloc_skb(1532)) != NULL))
+			if ((length > RX_COPYBREAK) &&
+			    ((newskb=dev_alloc_skb(1532)) != NULL))
 			{
 				skb=lp->rx_ring[rx_ring_tail].skb;
 				skb_put(skb, length);
diff --git a/drivers/net/3c59x.c b/drivers/net/3c59x.c
index 975e25b19ebe..78b7167a8ce3 100644
--- a/drivers/net/3c59x.c
+++ b/drivers/net/3c59x.c
@@ -1942,8 +1942,8 @@ vortex_error(struct net_device *dev, int status)
 	if (status & TxComplete) {			/* Really "TxError" for us. */
 		tx_status = ioread8(ioaddr + TxStatus);
 		/* Presumably a tx-timeout. We must merely re-enable. */
-		if (vortex_debug > 2
-			|| (tx_status != 0x88 && vortex_debug > 0)) {
+		if (vortex_debug > 2 ||
+		    (tx_status != 0x88 && vortex_debug > 0)) {
 			pr_err("%s: Transmit error, Tx status register %2.2x.\n",
 				   dev->name, tx_status);
 			if (tx_status == 0x82) {
@@ -2560,7 +2560,7 @@ boomerang_rx(struct net_device *dev)
 		struct sk_buff *skb;
 		entry = vp->dirty_rx % RX_RING_SIZE;
 		if (vp->rx_skbuff[entry] == NULL) {
-			skb = netdev_alloc_skb(dev, PKT_BUF_SZ + NET_IP_ALIGN);
+			skb = netdev_alloc_skb_ip_align(dev, PKT_BUF_SZ);
 			if (skb == NULL) {
 				static unsigned long last_jif;
 				if (time_after(jiffies, last_jif + 10 * HZ)) {
@@ -2572,7 +2572,6 @@ boomerang_rx(struct net_device *dev)
 				break;			/* Bad news!  */
 			}
 
-			skb_reserve(skb, NET_IP_ALIGN);
 			vp->rx_ring[entry].addr = cpu_to_le32(pci_map_single(VORTEX_PCI(vp), skb->data, PKT_BUF_SZ, PCI_DMA_FROMDEVICE));
 			vp->rx_skbuff[entry] = skb;
 		}
diff --git a/drivers/net/8139cp.c b/drivers/net/8139cp.c
index 83a1922e68e0..3f452bcbfb9e 100644
--- a/drivers/net/8139cp.c
+++ b/drivers/net/8139cp.c
@@ -549,14 +549,12 @@ rx_status_loop:
 			pr_debug("%s: rx slot %d status 0x%x len %d\n",
 			       dev->name, rx_tail, status, len);
 
-		new_skb = netdev_alloc_skb(dev, buflen + NET_IP_ALIGN);
+		new_skb = netdev_alloc_skb_ip_align(dev, buflen);
 		if (!new_skb) {
 			dev->stats.rx_dropped++;
 			goto rx_next;
 		}
 
-		skb_reserve(new_skb, NET_IP_ALIGN);
-
 		dma_unmap_single(&cp->pdev->dev, mapping,
 				 buflen, PCI_DMA_FROMDEVICE);
 
@@ -911,8 +909,8 @@ static void __cp_set_rx_mode (struct net_device *dev)
 		    AcceptBroadcast | AcceptMulticast | AcceptMyPhys |
 		    AcceptAllPhys;
 		mc_filter[1] = mc_filter[0] = 0xffffffff;
-	} else if ((dev->mc_count > multicast_filter_limit)
-		   || (dev->flags & IFF_ALLMULTI)) {
+	} else if ((dev->mc_count > multicast_filter_limit) ||
+		   (dev->flags & IFF_ALLMULTI)) {
 		/* Too many to filter perfectly -- accept all multicasts. */
 		rx_mode = AcceptBroadcast | AcceptMulticast | AcceptMyPhys;
 		mc_filter[1] = mc_filter[0] = 0xffffffff;
@@ -1057,12 +1055,10 @@ static int cp_refill_rx(struct cp_private *cp)
 		struct sk_buff *skb;
 		dma_addr_t mapping;
 
-		skb = netdev_alloc_skb(dev, cp->rx_buf_sz + NET_IP_ALIGN);
+		skb = netdev_alloc_skb_ip_align(dev, cp->rx_buf_sz);
 		if (!skb)
 			goto err_out;
 
-		skb_reserve(skb, NET_IP_ALIGN);
-
 		mapping = dma_map_single(&cp->pdev->dev, skb->data,
 					 cp->rx_buf_sz, PCI_DMA_FROMDEVICE);
 		cp->rx_skb[i] = skb;
diff --git a/drivers/net/8139too.c b/drivers/net/8139too.c
index 4a3628755026..25f7339daabd 100644
--- a/drivers/net/8139too.c
+++ b/drivers/net/8139too.c
@@ -1549,8 +1549,8 @@ static inline void rtl8139_thread_iter (struct net_device *dev,
 	mii_lpa = mdio_read (dev, tp->phys[0], MII_LPA);
 
 	if (!tp->mii.force_media && mii_lpa != 0xffff) {
-		int duplex = (mii_lpa & LPA_100FULL)
-		    || (mii_lpa & 0x01C0) == 0x0040;
+		int duplex = ((mii_lpa & LPA_100FULL) ||
+			      (mii_lpa & 0x01C0) == 0x0040);
 		if (tp->mii.full_duplex != duplex) {
 			tp->mii.full_duplex = duplex;
 
@@ -1936,8 +1936,8 @@ static int rtl8139_rx(struct net_device *dev, struct rtl8139_private *tp,
 		 RTL_R16 (RxBufAddr),
 		 RTL_R16 (RxBufPtr), RTL_R8 (ChipCmd));
 
-	while (netif_running(dev) && received < budget
-	       && (RTL_R8 (ChipCmd) & RxBufEmpty) == 0) {
+	while (netif_running(dev) && received < budget &&
+	       (RTL_R8 (ChipCmd) & RxBufEmpty) == 0) {
 		u32 ring_offset = cur_rx % RX_BUF_LEN;
 		u32 rx_status;
 		unsigned int pkt_size;
@@ -2004,9 +2004,8 @@ no_early_rx:
 		/* Malloc up new buffer, compatible with net-2e. */
 		/* Omit the four octet CRC from the length. */
 
-		skb = netdev_alloc_skb(dev, pkt_size + NET_IP_ALIGN);
+		skb = netdev_alloc_skb_ip_align(dev, pkt_size);
 		if (likely(skb)) {
-			skb_reserve (skb, NET_IP_ALIGN);	/* 16 byte align the IP fields. */
 #if RX_BUF_IDX == 3
 			wrap_copy(skb, rx_ring, ring_offset+4, pkt_size);
 #else
@@ -2522,8 +2521,8 @@ static void __set_rx_mode (struct net_device *dev)
 		    AcceptBroadcast | AcceptMulticast | AcceptMyPhys |
 		    AcceptAllPhys;
 		mc_filter[1] = mc_filter[0] = 0xffffffff;
-	} else if ((dev->mc_count > multicast_filter_limit)
-		   || (dev->flags & IFF_ALLMULTI)) {
+	} else if ((dev->mc_count > multicast_filter_limit) ||
+		   (dev->flags & IFF_ALLMULTI)) {
 		/* Too many to filter perfectly -- accept all multicasts. */
 		rx_mode = AcceptBroadcast | AcceptMulticast | AcceptMyPhys;
 		mc_filter[1] = mc_filter[0] = 0xffffffff;
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index b2f71f79baaf..0bbd5ae49862 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -1001,7 +1001,7 @@ config SMC911X
 
 config SMSC911X
 	tristate "SMSC LAN911x/LAN921x families embedded ethernet support"
-	depends on ARM || SUPERH || BLACKFIN
+	depends on ARM || SUPERH || BLACKFIN || MIPS
 	select CRC32
 	select MII
 	select PHYLIB
@@ -3235,7 +3235,7 @@ config VIRTIO_NET
 
 config VMXNET3
        tristate "VMware VMXNET3 ethernet driver"
-       depends on PCI && X86 && INET
+       depends on PCI && INET
        help
          This driver supports VMware's vmxnet3 virtual ethernet NIC.
          To compile this driver as a module, choose M here: the
diff --git a/drivers/net/appletalk/cops.c b/drivers/net/appletalk/cops.c
index b5dc7f550725..50cecf417471 100644
--- a/drivers/net/appletalk/cops.c
+++ b/drivers/net/appletalk/cops.c
@@ -328,7 +328,7 @@ static int __init cops_probe1(struct net_device *dev, int ioaddr)
 
 	/* Reserve any actual interrupt. */
 	if (dev->irq) {
-		retval = request_irq(dev->irq, &cops_interrupt, 0, dev->name, dev);
+		retval = request_irq(dev->irq, cops_interrupt, 0, dev->name, dev);
 		if (retval)
 			goto err_out;
 	}
diff --git a/drivers/net/appletalk/ipddp.c b/drivers/net/appletalk/ipddp.c
index aaf14d306a4a..eb0448b03f41 100644
--- a/drivers/net/appletalk/ipddp.c
+++ b/drivers/net/appletalk/ipddp.c
@@ -230,9 +230,9 @@ static int ipddp_delete(struct ipddp_route *rt)
 	spin_lock_bh(&ipddp_route_lock);
         while((tmp = *r) != NULL)
         {
-                if(tmp->ip == rt->ip
-                        && tmp->at.s_net == rt->at.s_net
-                        && tmp->at.s_node == rt->at.s_node)
+                if(tmp->ip == rt->ip &&
+		   tmp->at.s_net == rt->at.s_net &&
+		   tmp->at.s_node == rt->at.s_node)
                 {
                         *r = tmp->next;
 			spin_unlock_bh(&ipddp_route_lock);
@@ -255,9 +255,9 @@ static struct ipddp_route* __ipddp_find_route(struct ipddp_route *rt)
 
         for(f = ipddp_route_list; f != NULL; f = f->next)
         {
-                if(f->ip == rt->ip
-                        && f->at.s_net == rt->at.s_net
-                        && f->at.s_node == rt->at.s_node)
+                if(f->ip == rt->ip &&
+		   f->at.s_net == rt->at.s_net &&
+		   f->at.s_node == rt->at.s_node)
                         return (f);
         }
 
diff --git a/drivers/net/appletalk/ltpc.c b/drivers/net/appletalk/ltpc.c
index 08760baece7a..dbfbd3b7ff86 100644
--- a/drivers/net/appletalk/ltpc.c
+++ b/drivers/net/appletalk/ltpc.c
@@ -1158,7 +1158,7 @@ struct net_device * __init ltpc_probe(void)
 	}
 
 	/* grab it and don't let go :-) */
-	if (irq && request_irq( irq, &ltpc_interrupt, 0, "ltpc", dev) >= 0)
+	if (irq && request_irq( irq, ltpc_interrupt, 0, "ltpc", dev) >= 0)
 	{
 		(void) inb_p(io+7);  /* enable interrupts from board */
 		(void) inb_p(io+7);  /* and reset irq line */
diff --git a/drivers/net/arcnet/arc-rimi.c b/drivers/net/arcnet/arc-rimi.c
index e3082a9350fc..e6afab2455b1 100644
--- a/drivers/net/arcnet/arc-rimi.c
+++ b/drivers/net/arcnet/arc-rimi.c
@@ -156,7 +156,7 @@ static int __init arcrimi_found(struct net_device *dev)
 	}
 
 	/* reserve the irq */
-	if (request_irq(dev->irq, &arcnet_interrupt, 0, "arcnet (RIM I)", dev)) {
+	if (request_irq(dev->irq, arcnet_interrupt, 0, "arcnet (RIM I)", dev)) {
 		iounmap(p);
 		release_mem_region(dev->mem_start, MIRROR_SIZE);
 		BUGMSG(D_NORMAL, "Can't get IRQ %d!\n", dev->irq);
@@ -174,9 +174,9 @@ static int __init arcrimi_found(struct net_device *dev)
 	 * 2k (or there are no mirrors at all) but on some, it's 4k.
 	 */
 	mirror_size = MIRROR_SIZE;
-	if (readb(p) == TESTvalue
-	    && check_mirror(shmem - MIRROR_SIZE, MIRROR_SIZE) == 0
-	    && check_mirror(shmem - 2 * MIRROR_SIZE, MIRROR_SIZE) == 1)
+	if (readb(p) == TESTvalue &&
+	    check_mirror(shmem - MIRROR_SIZE, MIRROR_SIZE) == 0 &&
+	    check_mirror(shmem - 2 * MIRROR_SIZE, MIRROR_SIZE) == 1)
 		mirror_size = 2 * MIRROR_SIZE;
 
 	first_mirror = shmem - mirror_size;
diff --git a/drivers/net/arcnet/arcnet.c b/drivers/net/arcnet/arcnet.c
index 75a572560909..d8f029303754 100644
--- a/drivers/net/arcnet/arcnet.c
+++ b/drivers/net/arcnet/arcnet.c
@@ -301,8 +301,8 @@ static int choose_mtu(void)
 
 	/* choose the smallest MTU of all available encaps */
 	for (count = 0; count < 256; count++) {
-		if (arc_proto_map[count] != &arc_proto_null
-		    && arc_proto_map[count]->mtu < mtu) {
+		if (arc_proto_map[count] != &arc_proto_null &&
+		    arc_proto_map[count]->mtu < mtu) {
 			mtu = arc_proto_map[count]->mtu;
 		}
 	}
@@ -953,13 +953,13 @@ irqreturn_t arcnet_interrupt(int irq, void *dev_id)
 				 *    > RECON_THRESHOLD/min;
 				 * then print a warning message.
 				 */
-				if (!lp->network_down
-				    && (lp->last_recon - lp->first_recon) <= HZ * 60
-				  && lp->num_recons >= RECON_THRESHOLD) {
+				if (!lp->network_down &&
+				    (lp->last_recon - lp->first_recon) <= HZ * 60 &&
+				    lp->num_recons >= RECON_THRESHOLD) {
 					lp->network_down = 1;
 					BUGMSG(D_NORMAL, "many reconfigurations detected: cabling problem?\n");
-				} else if (!lp->network_down
-					   && lp->last_recon - lp->first_recon > HZ * 60) {
+				} else if (!lp->network_down &&
+					   lp->last_recon - lp->first_recon > HZ * 60) {
 					/* reset counters if we've gone for over a minute. */
 					lp->first_recon = lp->last_recon;
 					lp->num_recons = 1;
diff --git a/drivers/net/arcnet/com20020.c b/drivers/net/arcnet/com20020.c
index 651275a5f3d2..0a74f21409c5 100644
--- a/drivers/net/arcnet/com20020.c
+++ b/drivers/net/arcnet/com20020.c
@@ -200,7 +200,7 @@ int com20020_found(struct net_device *dev, int shared)
 	outb(dev->dev_addr[0], _XREG);
 
 	/* reserve the irq */
-	if (request_irq(dev->irq, &arcnet_interrupt, shared,
+	if (request_irq(dev->irq, arcnet_interrupt, shared,
 			"arcnet (COM20020)", dev)) {
 		BUGMSG(D_NORMAL, "Can't get IRQ %d!\n", dev->irq);
 		return -ENODEV;
diff --git a/drivers/net/arcnet/com90io.c b/drivers/net/arcnet/com90io.c
index 89de29b3b1dc..28dea518d554 100644
--- a/drivers/net/arcnet/com90io.c
+++ b/drivers/net/arcnet/com90io.c
@@ -238,7 +238,7 @@ static int __init com90io_found(struct net_device *dev)
 	int err;
 
 	/* Reserve the irq */
-	if (request_irq(dev->irq, &arcnet_interrupt, 0, "arcnet (COM90xx-IO)", dev)) {
+	if (request_irq(dev->irq, arcnet_interrupt, 0, "arcnet (COM90xx-IO)", dev)) {
 		BUGMSG(D_NORMAL, "Can't get IRQ %d!\n", dev->irq);
 		return -ENODEV;
 	}
diff --git a/drivers/net/arcnet/com90xx.c b/drivers/net/arcnet/com90xx.c
index d762fe46251e..112e230cb13d 100644
--- a/drivers/net/arcnet/com90xx.c
+++ b/drivers/net/arcnet/com90xx.c
@@ -501,7 +501,7 @@ static int __init com90xx_found(int ioaddr, int airq, u_long shmem, void __iomem
 		goto err_free_dev;
 
 	/* reserve the irq */
-	if (request_irq(airq, &arcnet_interrupt, 0, "arcnet (90xx)", dev)) {
+	if (request_irq(airq, arcnet_interrupt, 0, "arcnet (90xx)", dev)) {
 		BUGMSG(D_NORMAL, "Can't get IRQ %d!\n", airq);
 		goto err_release_mem;
 	}
diff --git a/drivers/net/arm/ks8695net.c b/drivers/net/arm/ks8695net.c
index 2a7b7745cc55..be256b34cea8 100644
--- a/drivers/net/arm/ks8695net.c
+++ b/drivers/net/arm/ks8695net.c
@@ -35,11 +35,13 @@
 
 #include <mach/regs-switch.h>
 #include <mach/regs-misc.h>
+#include <asm/mach/irq.h>
+#include <mach/regs-irq.h>
 
 #include "ks8695net.h"
 
 #define MODULENAME	"ks8695_ether"
-#define MODULEVERSION	"1.01"
+#define MODULEVERSION	"1.02"
 
 /*
  * Transmit and device reset timeout, default 5 seconds.
@@ -95,6 +97,9 @@ struct ks8695_skbuff {
 #define MAX_RX_DESC 16
 #define MAX_RX_DESC_MASK 0xf
 
+/*napi_weight have better more than rx DMA buffers*/
+#define NAPI_WEIGHT   64
+
 #define MAX_RXBUF_SIZE 0x700
 
 #define TX_RING_DMA_SIZE (sizeof(struct tx_ring_desc) * MAX_TX_DESC)
@@ -120,6 +125,7 @@ enum ks8695_dtype {
  *	@dev: The platform device object for this interface
  *	@dtype: The type of this device
  *	@io_regs: The ioremapped registers for this interface
+ *      @napi : Add support NAPI for Rx
  *	@rx_irq_name: The textual name of the RX IRQ from the platform data
  *	@tx_irq_name: The textual name of the TX IRQ from the platform data
  *	@link_irq_name: The textual name of the link IRQ from the
@@ -143,6 +149,7 @@ enum ks8695_dtype {
  *	@rx_ring_dma: The DMA mapped equivalent of rx_ring
  *	@rx_buffers: The sk_buff mappings for the RX ring
  *	@next_rx_desc_read: The next RX descriptor to read from on IRQ
+ *      @rx_lock: A lock to protect Rx irq function
  *	@msg_enable: The flags for which messages to emit
  */
 struct ks8695_priv {
@@ -152,6 +159,8 @@ struct ks8695_priv {
 	enum ks8695_dtype dtype;
 	void __iomem *io_regs;
 
+	struct napi_struct	napi;
+
 	const char *rx_irq_name, *tx_irq_name, *link_irq_name;
 	int rx_irq, tx_irq, link_irq;
 
@@ -172,6 +181,7 @@ struct ks8695_priv {
 	dma_addr_t rx_ring_dma;
 	struct ks8695_skbuff rx_buffers[MAX_RX_DESC];
 	int next_rx_desc_read;
+	spinlock_t rx_lock;
 
 	int msg_enable;
 };
@@ -392,29 +402,74 @@ ks8695_tx_irq(int irq, void *dev_id)
 }
 
 /**
+ *	ks8695_get_rx_enable_bit - Get rx interrupt enable/status bit
+ *	@ksp: Private data for the KS8695 Ethernet
+ *
+ *    For KS8695 document:
+ *    Interrupt Enable Register (offset 0xE204)
+ *        Bit29 : WAN MAC Receive Interrupt Enable
+ *        Bit16 : LAN MAC Receive Interrupt Enable
+ *    Interrupt Status Register (Offset 0xF208)
+ *        Bit29: WAN MAC Receive Status
+ *        Bit16: LAN MAC Receive Status
+ *    So, this Rx interrrupt enable/status bit number is equal
+ *    as Rx IRQ number.
+ */
+static inline u32 ks8695_get_rx_enable_bit(struct ks8695_priv *ksp)
+{
+	return ksp->rx_irq;
+}
+
+/**
  *	ks8695_rx_irq - Receive IRQ handler
  *	@irq: The IRQ which went off (ignored)
  *	@dev_id: The net_device for the interrupt
  *
- *	Process the RX ring, passing any received packets up to the
- *	host.  If we received anything other than errors, we then
- *	refill the ring.
+ *	Inform NAPI that packet reception needs to be scheduled
  */
+
 static irqreturn_t
 ks8695_rx_irq(int irq, void *dev_id)
 {
 	struct net_device *ndev = (struct net_device *)dev_id;
 	struct ks8695_priv *ksp = netdev_priv(ndev);
+
+	spin_lock(&ksp->rx_lock);
+
+	if (napi_schedule_prep(&ksp->napi)) {
+		unsigned long status = readl(KS8695_IRQ_VA + KS8695_INTEN);
+		unsigned long mask_bit = 1 << ks8695_get_rx_enable_bit(ksp);
+		/*disable rx interrupt*/
+		status &= ~mask_bit;
+		writel(status , KS8695_IRQ_VA + KS8695_INTEN);
+		__napi_schedule(&ksp->napi);
+	}
+
+	spin_unlock(&ksp->rx_lock);
+	return IRQ_HANDLED;
+}
+
+/**
+ *	ks8695_rx - Receive packets  called by NAPI poll method
+ *	@ksp: Private data for the KS8695 Ethernet
+ *	@budget: The max packets would be receive
+ */
+
+static int ks8695_rx(struct ks8695_priv *ksp, int budget)
+{
+	struct net_device *ndev = ksp->ndev;
 	struct sk_buff *skb;
 	int buff_n;
 	u32 flags;
 	int pktlen;
 	int last_rx_processed = -1;
+	int received = 0;
 
 	buff_n = ksp->next_rx_desc_read;
-	do {
-		if (ksp->rx_buffers[buff_n].skb &&
-		    !(ksp->rx_ring[buff_n].status & cpu_to_le32(RDES_OWN))) {
+	while (received < budget
+			&& ksp->rx_buffers[buff_n].skb
+			&& (!(ksp->rx_ring[buff_n].status &
+					cpu_to_le32(RDES_OWN)))) {
 			rmb();
 			flags = le32_to_cpu(ksp->rx_ring[buff_n].status);
 			/* Found an SKB which we own, this means we
@@ -464,7 +519,7 @@ ks8695_rx_irq(int irq, void *dev_id)
 			/* Relinquish the SKB to the network layer */
 			skb_put(skb, pktlen);
 			skb->protocol = eth_type_trans(skb, ndev);
-			netif_rx(skb);
+			netif_receive_skb(skb);
 
 			/* Record stats */
 			ndev->stats.rx_packets++;
@@ -478,29 +533,55 @@ rx_failure:
 			/* Give the ring entry back to the hardware */
 			ksp->rx_ring[buff_n].status = cpu_to_le32(RDES_OWN);
 rx_finished:
+			received++;
 			/* And note this as processed so we can start
 			 * from here next time
 			 */
 			last_rx_processed = buff_n;
-		} else {
-			/* Ran out of things to process, stop now */
-			break;
-		}
-		buff_n = (buff_n + 1) & MAX_RX_DESC_MASK;
-	} while (buff_n != ksp->next_rx_desc_read);
-
-	/* And note which RX descriptor we last did anything with */
-	if (likely(last_rx_processed != -1))
-		ksp->next_rx_desc_read =
-			(last_rx_processed + 1) & MAX_RX_DESC_MASK;
-
+			buff_n = (buff_n + 1) & MAX_RX_DESC_MASK;
+			/*And note which RX descriptor we last did */
+			if (likely(last_rx_processed != -1))
+				ksp->next_rx_desc_read =
+					(last_rx_processed + 1) &
+					MAX_RX_DESC_MASK;
+	}
 	/* And refill the buffers */
 	ks8695_refill_rxbuffers(ksp);
 
-	/* Kick the RX DMA engine, in case it became suspended */
+	/* Kick the RX DMA engine, in case it became
+	 *  suspended */
 	ks8695_writereg(ksp, KS8695_DRSC, 0);
+	return received;
+}
 
-	return IRQ_HANDLED;
+
+/**
+ *	ks8695_poll - Receive packet by NAPI poll method
+ *	@ksp: Private data for the KS8695 Ethernet
+ *	@budget: The remaining number packets for network subsystem
+ *
+ *     Invoked by the network core when it requests for new
+ *     packets from the driver
+ */
+static int ks8695_poll(struct napi_struct *napi, int budget)
+{
+	struct ks8695_priv *ksp = container_of(napi, struct ks8695_priv, napi);
+	unsigned long  work_done;
+
+	unsigned long isr = readl(KS8695_IRQ_VA + KS8695_INTEN);
+	unsigned long mask_bit = 1 << ks8695_get_rx_enable_bit(ksp);
+
+	work_done = ks8695_rx(ksp, budget);
+
+	if (work_done < budget) {
+		unsigned long flags;
+		spin_lock_irqsave(&ksp->rx_lock, flags);
+		/*enable rx interrupt*/
+		writel(isr | mask_bit, KS8695_IRQ_VA + KS8695_INTEN);
+		__napi_complete(napi);
+		spin_unlock_irqrestore(&ksp->rx_lock, flags);
+	}
+	return work_done;
 }
 
 /**
@@ -1253,6 +1334,7 @@ ks8695_stop(struct net_device *ndev)
 	struct ks8695_priv *ksp = netdev_priv(ndev);
 
 	netif_stop_queue(ndev);
+	napi_disable(&ksp->napi);
 	netif_carrier_off(ndev);
 
 	ks8695_shutdown(ksp);
@@ -1287,6 +1369,7 @@ ks8695_open(struct net_device *ndev)
 		return ret;
 	}
 
+	napi_enable(&ksp->napi);
 	netif_start_queue(ndev);
 
 	return 0;
@@ -1472,6 +1555,8 @@ ks8695_probe(struct platform_device *pdev)
 	SET_ETHTOOL_OPS(ndev, &ks8695_ethtool_ops);
 	ndev->watchdog_timeo	 = msecs_to_jiffies(watchdog);
 
+	netif_napi_add(ndev, &ksp->napi, ks8695_poll, NAPI_WEIGHT);
+
 	/* Retrieve the default MAC addr from the chip. */
 	/* The bootloader should have left it in there for us. */
 
@@ -1505,6 +1590,7 @@ ks8695_probe(struct platform_device *pdev)
 
 	/* And initialise the queue's lock */
 	spin_lock_init(&ksp->txq_lock);
+	spin_lock_init(&ksp->rx_lock);
 
 	/* Specify the RX DMA ring buffer */
 	ksp->rx_ring = ksp->ring_base + TX_RING_DMA_SIZE;
@@ -1626,6 +1712,7 @@ ks8695_drv_remove(struct platform_device *pdev)
 	struct ks8695_priv *ksp = netdev_priv(ndev);
 
 	platform_set_drvdata(pdev, NULL);
+	netif_napi_del(&ksp->napi);
 
 	unregister_netdev(ndev);
 	ks8695_release_device(ksp);
diff --git a/drivers/net/arm/w90p910_ether.c b/drivers/net/arm/w90p910_ether.c
index 25e2627eb118..b7f3866d546f 100644
--- a/drivers/net/arm/w90p910_ether.c
+++ b/drivers/net/arm/w90p910_ether.c
@@ -160,8 +160,8 @@ struct  w90p910_ether {
 	struct mii_if_info mii;
 	struct timer_list check_timer;
 	void __iomem *reg;
-	unsigned int rxirq;
-	unsigned int txirq;
+	int rxirq;
+	int txirq;
 	unsigned int cur_tx;
 	unsigned int cur_rx;
 	unsigned int finish_tx;
diff --git a/drivers/net/at1700.c b/drivers/net/at1700.c
index 544d5af6950e..b14f4799d5d1 100644
--- a/drivers/net/at1700.c
+++ b/drivers/net/at1700.c
@@ -350,13 +350,13 @@ static int __init at1700_probe1(struct net_device *dev, int ioaddr)
 	slot = -1;
 	/* We must check for the EEPROM-config boards first, else accessing
 	   IOCONFIG0 will move the board! */
-	if (at1700_probe_list[inb(ioaddr + IOCONFIG1) & 0x07] == ioaddr
-		&& read_eeprom(ioaddr, 4) == 0x0000
-		&& (read_eeprom(ioaddr, 5) & 0xff00) == 0xF400)
+	if (at1700_probe_list[inb(ioaddr + IOCONFIG1) & 0x07] == ioaddr &&
+	    read_eeprom(ioaddr, 4) == 0x0000 &&
+	    (read_eeprom(ioaddr, 5) & 0xff00) == 0xF400)
 		is_at1700 = 1;
-	else if (inb(ioaddr   + SAPROM    ) == 0x00
-		&& inb(ioaddr + SAPROM + 1) == 0x00
-		&& inb(ioaddr + SAPROM + 2) == 0x0e)
+	else if (inb(ioaddr + SAPROM    ) == 0x00 &&
+		 inb(ioaddr + SAPROM + 1) == 0x00 &&
+		 inb(ioaddr + SAPROM + 2) == 0x0e)
 		is_fmv18x = 1;
 	else {
 		goto err_out;
@@ -468,7 +468,7 @@ found:
 	lp->jumpered = is_fmv18x;
 	lp->mca_slot = slot;
 	/* Snarf the interrupt vector now. */
-	ret = request_irq(irq, &net_interrupt, 0, DRV_NAME, dev);
+	ret = request_irq(irq, net_interrupt, 0, DRV_NAME, dev);
 	if (ret) {
 		printk(KERN_ERR "AT1700 at %#3x is unusable due to a "
 		       "conflict on IRQ %d.\n",
@@ -839,8 +839,8 @@ set_rx_mode(struct net_device *dev)
 	if (dev->flags & IFF_PROMISC) {
 		memset(mc_filter, 0xff, sizeof(mc_filter));
 		outb(3, ioaddr + RX_MODE);	/* Enable promiscuous mode */
-	} else if (dev->mc_count > MC_FILTERBREAK
-			   ||  (dev->flags & IFF_ALLMULTI)) {
+	} else if (dev->mc_count > MC_FILTERBREAK ||
+			   (dev->flags & IFF_ALLMULTI)) {
 		/* Too many to filter perfectly -- accept all multicasts. */
 		memset(mc_filter, 0xff, sizeof(mc_filter));
 		outb(2, ioaddr + RX_MODE);	/* Use normal mode. */
diff --git a/drivers/net/atarilance.c b/drivers/net/atarilance.c
index 0c0deceb6938..c5721cb38265 100644
--- a/drivers/net/atarilance.c
+++ b/drivers/net/atarilance.c
@@ -930,8 +930,8 @@ static irqreturn_t lance_interrupt( int irq, void *dev_id )
 			}
 #endif
 
-			if (lp->tx_full && (netif_queue_stopped(dev))
-				&& dirty_tx > lp->cur_tx - TX_RING_SIZE + 2) {
+			if (lp->tx_full && (netif_queue_stopped(dev)) &&
+				dirty_tx > lp->cur_tx - TX_RING_SIZE + 2) {
 				/* The ring is no longer full, clear tbusy. */
 				lp->tx_full = 0;
 				netif_wake_queue (dev);
diff --git a/drivers/net/atl1c/atl1c.h b/drivers/net/atl1c/atl1c.h
index 2a1120ad2e74..a348a22551d9 100644
--- a/drivers/net/atl1c/atl1c.h
+++ b/drivers/net/atl1c/atl1c.h
@@ -470,12 +470,28 @@ struct atl1c_ring_header {
 struct atl1c_buffer {
 	struct sk_buff *skb;	/* socket buffer */
 	u16 length;		/* rx buffer length */
-	u16 state;		/* state of buffer */
-#define ATL1_BUFFER_FREE	0
-#define ATL1_BUFFER_BUSY	1
+	u16 flags;		/* information of buffer */
+#define ATL1C_BUFFER_FREE		0x0001
+#define ATL1C_BUFFER_BUSY		0x0002
+#define ATL1C_BUFFER_STATE_MASK		0x0003
+
+#define ATL1C_PCIMAP_SINGLE		0x0004
+#define ATL1C_PCIMAP_PAGE		0x0008
+#define ATL1C_PCIMAP_TYPE_MASK		0x000C
+
 	dma_addr_t dma;
 };
 
+#define ATL1C_SET_BUFFER_STATE(buff, state) do {	\
+	((buff)->flags) &= ~ATL1C_BUFFER_STATE_MASK;	\
+	((buff)->flags) |= (state);			\
+	} while (0)
+
+#define ATL1C_SET_PCIMAP_TYPE(buff, type) do {		\
+	((buff)->flags) &= ~ATL1C_PCIMAP_TYPE_MASK;	\
+	((buff)->flags) |= (type);			\
+	} while (0)
+
 /* transimit packet descriptor (tpd) ring */
 struct atl1c_tpd_ring {
 	void *desc;		/* descriptor ring virtual address */
diff --git a/drivers/net/atl1c/atl1c_main.c b/drivers/net/atl1c/atl1c_main.c
index 1372e9a99f5b..1e2f57d4c367 100644
--- a/drivers/net/atl1c/atl1c_main.c
+++ b/drivers/net/atl1c/atl1c_main.c
@@ -710,6 +710,29 @@ static int __devinit atl1c_sw_init(struct atl1c_adapter *adapter)
 	return 0;
 }
 
+static inline void atl1c_clean_buffer(struct pci_dev *pdev,
+				struct atl1c_buffer *buffer_info, int in_irq)
+{
+	if (buffer_info->flags & ATL1C_BUFFER_FREE)
+		return;
+	if (buffer_info->dma) {
+		if (buffer_info->flags & ATL1C_PCIMAP_SINGLE)
+			pci_unmap_single(pdev, buffer_info->dma,
+					buffer_info->length, PCI_DMA_TODEVICE);
+		else if (buffer_info->flags & ATL1C_PCIMAP_PAGE)
+			pci_unmap_page(pdev, buffer_info->dma,
+					buffer_info->length, PCI_DMA_TODEVICE);
+	}
+	if (buffer_info->skb) {
+		if (in_irq)
+			dev_kfree_skb_irq(buffer_info->skb);
+		else
+			dev_kfree_skb(buffer_info->skb);
+	}
+	buffer_info->dma = 0;
+	buffer_info->skb = NULL;
+	ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_FREE);
+}
 /*
  * atl1c_clean_tx_ring - Free Tx-skb
  * @adapter: board private structure
@@ -725,22 +748,12 @@ static void atl1c_clean_tx_ring(struct atl1c_adapter *adapter,
 	ring_count = tpd_ring->count;
 	for (index = 0; index < ring_count; index++) {
 		buffer_info = &tpd_ring->buffer_info[index];
-		if (buffer_info->state == ATL1_BUFFER_FREE)
-			continue;
-		if (buffer_info->dma)
-			pci_unmap_single(pdev, buffer_info->dma,
-					buffer_info->length,
-					PCI_DMA_TODEVICE);
-		if (buffer_info->skb)
-			dev_kfree_skb(buffer_info->skb);
-		buffer_info->dma = 0;
-		buffer_info->skb = NULL;
-		buffer_info->state = ATL1_BUFFER_FREE;
+		atl1c_clean_buffer(pdev, buffer_info, 0);
 	}
 
 	/* Zero out Tx-buffers */
 	memset(tpd_ring->desc, 0, sizeof(struct atl1c_tpd_desc) *
-				ring_count);
+		ring_count);
 	atomic_set(&tpd_ring->next_to_clean, 0);
 	tpd_ring->next_to_use = 0;
 }
@@ -760,16 +773,7 @@ static void atl1c_clean_rx_ring(struct atl1c_adapter *adapter)
 	for (i = 0; i < adapter->num_rx_queues; i++) {
 		for (j = 0; j < rfd_ring[i].count; j++) {
 			buffer_info = &rfd_ring[i].buffer_info[j];
-			if (buffer_info->state == ATL1_BUFFER_FREE)
-				continue;
-			if (buffer_info->dma)
-				pci_unmap_single(pdev, buffer_info->dma,
-						buffer_info->length,
-						PCI_DMA_FROMDEVICE);
-			if (buffer_info->skb)
-				dev_kfree_skb(buffer_info->skb);
-			buffer_info->state = ATL1_BUFFER_FREE;
-			buffer_info->skb = NULL;
+			atl1c_clean_buffer(pdev, buffer_info, 0);
 		}
 		/* zero out the descriptor ring */
 		memset(rfd_ring[i].desc, 0, rfd_ring[i].size);
@@ -796,7 +800,8 @@ static void atl1c_init_ring_ptrs(struct atl1c_adapter *adapter)
 		atomic_set(&tpd_ring[i].next_to_clean, 0);
 		buffer_info = tpd_ring[i].buffer_info;
 		for (j = 0; j < tpd_ring->count; j++)
-			buffer_info[i].state = ATL1_BUFFER_FREE;
+			ATL1C_SET_BUFFER_STATE(&buffer_info[i],
+					ATL1C_BUFFER_FREE);
 	}
 	for (i = 0; i < adapter->num_rx_queues; i++) {
 		rfd_ring[i].next_to_use = 0;
@@ -805,7 +810,7 @@ static void atl1c_init_ring_ptrs(struct atl1c_adapter *adapter)
 		rrd_ring[i].next_to_clean = 0;
 		for (j = 0; j < rfd_ring[i].count; j++) {
 			buffer_info = &rfd_ring[i].buffer_info[j];
-			buffer_info->state = ATL1_BUFFER_FREE;
+			ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_FREE);
 		}
 	}
 }
@@ -1447,6 +1452,7 @@ static bool atl1c_clean_tx_irq(struct atl1c_adapter *adapter,
 	struct atl1c_tpd_ring *tpd_ring = (struct atl1c_tpd_ring *)
 				&adapter->tpd_ring[type];
 	struct atl1c_buffer *buffer_info;
+	struct pci_dev *pdev = adapter->pdev;
 	u16 next_to_clean = atomic_read(&tpd_ring->next_to_clean);
 	u16 hw_next_to_clean;
 	u16 shift;
@@ -1462,16 +1468,7 @@ static bool atl1c_clean_tx_irq(struct atl1c_adapter *adapter,
 
 	while (next_to_clean != hw_next_to_clean) {
 		buffer_info = &tpd_ring->buffer_info[next_to_clean];
-		if (buffer_info->state == ATL1_BUFFER_BUSY) {
-			pci_unmap_page(adapter->pdev, buffer_info->dma,
-					buffer_info->length, PCI_DMA_TODEVICE);
-			buffer_info->dma = 0;
-			if (buffer_info->skb) {
-				dev_kfree_skb_irq(buffer_info->skb);
-				buffer_info->skb = NULL;
-			}
-			buffer_info->state = ATL1_BUFFER_FREE;
-		}
+		atl1c_clean_buffer(pdev, buffer_info, 1);
 		if (++next_to_clean == tpd_ring->count)
 			next_to_clean = 0;
 		atomic_set(&tpd_ring->next_to_clean, next_to_clean);
@@ -1587,7 +1584,7 @@ static int atl1c_alloc_rx_buffer(struct atl1c_adapter *adapter, const int ringid
 	buffer_info = &rfd_ring->buffer_info[rfd_next_to_use];
 	next_info = &rfd_ring->buffer_info[next_next];
 
-	while (next_info->state == ATL1_BUFFER_FREE) {
+	while (next_info->flags & ATL1C_BUFFER_FREE) {
 		rfd_desc = ATL1C_RFD_DESC(rfd_ring, rfd_next_to_use);
 
 		skb = dev_alloc_skb(adapter->rx_buffer_len);
@@ -1603,12 +1600,13 @@ static int atl1c_alloc_rx_buffer(struct atl1c_adapter *adapter, const int ringid
 		 * the 14 byte MAC header is removed
 		 */
 		vir_addr = skb->data;
-		buffer_info->state = ATL1_BUFFER_BUSY;
+		ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_BUSY);
 		buffer_info->skb = skb;
 		buffer_info->length = adapter->rx_buffer_len;
 		buffer_info->dma = pci_map_single(pdev, vir_addr,
 						buffer_info->length,
 						PCI_DMA_FROMDEVICE);
+		ATL1C_SET_PCIMAP_TYPE(buffer_info, ATL1C_PCIMAP_SINGLE);
 		rfd_desc->buffer_addr = cpu_to_le64(buffer_info->dma);
 		rfd_next_to_use = next_next;
 		if (++next_next == rfd_ring->count)
@@ -1653,7 +1651,8 @@ static void atl1c_clean_rfd(struct atl1c_rfd_ring *rfd_ring,
 			RRS_RX_RFD_INDEX_MASK;
 	for (i = 0; i < num; i++) {
 		buffer_info[rfd_index].skb = NULL;
-		buffer_info[rfd_index].state = ATL1_BUFFER_FREE;
+		ATL1C_SET_BUFFER_STATE(&buffer_info[rfd_index],
+					ATL1C_BUFFER_FREE);
 		if (++rfd_index == rfd_ring->count)
 			rfd_index = 0;
 	}
@@ -1967,7 +1966,8 @@ static void atl1c_tx_map(struct atl1c_adapter *adapter,
 		buffer_info->length = map_len;
 		buffer_info->dma = pci_map_single(adapter->pdev,
 					skb->data, hdr_len, PCI_DMA_TODEVICE);
-		buffer_info->state = ATL1_BUFFER_BUSY;
+		ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_BUSY);
+		ATL1C_SET_PCIMAP_TYPE(buffer_info, ATL1C_PCIMAP_SINGLE);
 		mapped_len += map_len;
 		use_tpd->buffer_addr = cpu_to_le64(buffer_info->dma);
 		use_tpd->buffer_len = cpu_to_le16(buffer_info->length);
@@ -1981,16 +1981,14 @@ static void atl1c_tx_map(struct atl1c_adapter *adapter,
 		else {
 			use_tpd = atl1c_get_tpd(adapter, type);
 			memcpy(use_tpd, tpd, sizeof(struct atl1c_tpd_desc));
-			use_tpd = atl1c_get_tpd(adapter, type);
-			memcpy(use_tpd, tpd, sizeof(struct atl1c_tpd_desc));
 		}
 		buffer_info = atl1c_get_tx_buffer(adapter, use_tpd);
 		buffer_info->length = buf_len - mapped_len;
 		buffer_info->dma =
 			pci_map_single(adapter->pdev, skb->data + mapped_len,
 					buffer_info->length, PCI_DMA_TODEVICE);
-		buffer_info->state = ATL1_BUFFER_BUSY;
-
+		ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_BUSY);
+		ATL1C_SET_PCIMAP_TYPE(buffer_info, ATL1C_PCIMAP_SINGLE);
 		use_tpd->buffer_addr = cpu_to_le64(buffer_info->dma);
 		use_tpd->buffer_len  = cpu_to_le16(buffer_info->length);
 	}
@@ -2010,8 +2008,8 @@ static void atl1c_tx_map(struct atl1c_adapter *adapter,
 					frag->page_offset,
 					buffer_info->length,
 					PCI_DMA_TODEVICE);
-		buffer_info->state = ATL1_BUFFER_BUSY;
-
+		ATL1C_SET_BUFFER_STATE(buffer_info, ATL1C_BUFFER_BUSY);
+		ATL1C_SET_PCIMAP_TYPE(buffer_info, ATL1C_PCIMAP_PAGE);
 		use_tpd->buffer_addr = cpu_to_le64(buffer_info->dma);
 		use_tpd->buffer_len  = cpu_to_le16(buffer_info->length);
 	}
@@ -2137,7 +2135,7 @@ static int atl1c_request_irq(struct atl1c_adapter *adapter)
 
 	if (!adapter->have_msi)
 		flags |= IRQF_SHARED;
-	err = request_irq(adapter->pdev->irq, &atl1c_intr, flags,
+	err = request_irq(adapter->pdev->irq, atl1c_intr, flags,
 			netdev->name, netdev);
 	if (err) {
 		if (netif_msg_ifup(adapter))
diff --git a/drivers/net/atl1e/atl1e_ethtool.c b/drivers/net/atl1e/atl1e_ethtool.c
index 60edb9f232bb..a76006c1bc6b 100644
--- a/drivers/net/atl1e/atl1e_ethtool.c
+++ b/drivers/net/atl1e/atl1e_ethtool.c
@@ -131,11 +131,6 @@ static int atl1e_set_settings(struct net_device *netdev,
 	return 0;
 }
 
-static u32 atl1e_get_tx_csum(struct net_device *netdev)
-{
-	return (netdev->features & NETIF_F_HW_CSUM) != 0;
-}
-
 static u32 atl1e_get_msglevel(struct net_device *netdev)
 {
 #ifdef DBG
@@ -145,10 +140,6 @@ static u32 atl1e_get_msglevel(struct net_device *netdev)
 #endif
 }
 
-static void atl1e_set_msglevel(struct net_device *netdev, u32 data)
-{
-}
-
 static int atl1e_get_regs_len(struct net_device *netdev)
 {
 	return AT_REGS_LEN * sizeof(u32);
@@ -387,18 +378,14 @@ static const struct ethtool_ops atl1e_ethtool_ops = {
 	.get_wol                = atl1e_get_wol,
 	.set_wol                = atl1e_set_wol,
 	.get_msglevel           = atl1e_get_msglevel,
-	.set_msglevel           = atl1e_set_msglevel,
 	.nway_reset             = atl1e_nway_reset,
 	.get_link               = ethtool_op_get_link,
 	.get_eeprom_len         = atl1e_get_eeprom_len,
 	.get_eeprom             = atl1e_get_eeprom,
 	.set_eeprom             = atl1e_set_eeprom,
-	.get_tx_csum            = atl1e_get_tx_csum,
-	.get_sg                 = ethtool_op_get_sg,
+	.set_tx_csum            = ethtool_op_set_tx_hw_csum,
 	.set_sg                 = ethtool_op_set_sg,
-#ifdef NETIF_F_TSO
-	.get_tso                = ethtool_op_get_tso,
-#endif
+	.set_tso                = ethtool_op_set_tso,
 };
 
 void atl1e_set_ethtool_ops(struct net_device *netdev)
diff --git a/drivers/net/atl1e/atl1e_main.c b/drivers/net/atl1e/atl1e_main.c
index 955da733c2ad..08f8c0969e9b 100644
--- a/drivers/net/atl1e/atl1e_main.c
+++ b/drivers/net/atl1e/atl1e_main.c
@@ -1433,14 +1433,12 @@ static void atl1e_clean_rx_irq(struct atl1e_adapter *adapter, u8 que,
 
 			packet_size = ((prrs->word1 >> RRS_PKT_SIZE_SHIFT) &
 					RRS_PKT_SIZE_MASK) - 4; /* CRC */
-			skb = netdev_alloc_skb(netdev,
-					       packet_size + NET_IP_ALIGN);
+			skb = netdev_alloc_skb_ip_align(netdev, packet_size);
 			if (skb == NULL) {
 				dev_warn(&pdev->dev, "%s: Memory squeeze,"
 					"deferring packet.\n", netdev->name);
 				goto skip_pkt;
 			}
-			skb_reserve(skb, NET_IP_ALIGN);
 			skb->dev = netdev;
 			memcpy(skb->data, (u8 *)(prrs + 1), packet_size);
 			skb_put(skb, packet_size);
@@ -1666,41 +1664,6 @@ static int atl1e_tso_csum(struct atl1e_adapter *adapter,
 			}
 			return 0;
 		}
-
-		if (offload_type & SKB_GSO_TCPV6) {
-			real_len = (((unsigned char *)ipv6_hdr(skb) - skb->data)
-					+ ntohs(ipv6_hdr(skb)->payload_len));
-			if (real_len < skb->len)
-				pskb_trim(skb, real_len);
-
-			/* check payload == 0 byte ? */
-			hdr_len = (skb_transport_offset(skb) + tcp_hdrlen(skb));
-			if (unlikely(skb->len == hdr_len)) {
-				/* only xsum need */
-				dev_warn(&pdev->dev,
-					"IPV6 tso with zero data??\n");
-				goto check_sum;
-			} else {
-				tcp_hdr(skb)->check = ~csum_ipv6_magic(
-						&ipv6_hdr(skb)->saddr,
-						&ipv6_hdr(skb)->daddr,
-						0, IPPROTO_TCP, 0);
-				tpd->word3 |= 1 << TPD_IP_VERSION_SHIFT;
-				hdr_len >>= 1;
-				tpd->word3 |= (hdr_len & TPD_V6_IPHLLO_MASK) <<
-					TPD_V6_IPHLLO_SHIFT;
-				tpd->word3 |= ((hdr_len >> 3) &
-					TPD_V6_IPHLHI_MASK) <<
-					TPD_V6_IPHLHI_SHIFT;
-				tpd->word3 |= (tcp_hdrlen(skb) >> 2 &
-					TPD_TCPHDRLEN_MASK) <<
-					TPD_TCPHDRLEN_SHIFT;
-				tpd->word3 |= ((skb_shinfo(skb)->gso_size) &
-					TPD_MSS_MASK) << TPD_MSS_SHIFT;
-					tpd->word3 |= 1 << TPD_SEGMENT_EN_SHIFT;
-			}
-		}
-		return 0;
 	}
 
 check_sum:
@@ -1932,7 +1895,7 @@ static int atl1e_request_irq(struct atl1e_adapter *adapter)
 
 	if (!adapter->have_msi)
 		flags |= IRQF_SHARED;
-	err = request_irq(adapter->pdev->irq, &atl1e_intr, flags,
+	err = request_irq(adapter->pdev->irq, atl1e_intr, flags,
 			netdev->name, netdev);
 	if (err) {
 		dev_dbg(&pdev->dev,
@@ -2289,7 +2252,6 @@ static int atl1e_init_netdev(struct net_device *netdev, struct pci_dev *pdev)
 		NETIF_F_HW_VLAN_TX | NETIF_F_HW_VLAN_RX;
 	netdev->features |= NETIF_F_LLTX;
 	netdev->features |= NETIF_F_TSO;
-	netdev->features |= NETIF_F_TSO6;
 
 	return 0;
 }
diff --git a/drivers/net/atlx/atl1.c b/drivers/net/atlx/atl1.c
index 00569dc1313c..b6cf3263127c 100644
--- a/drivers/net/atlx/atl1.c
+++ b/drivers/net/atlx/atl1.c
@@ -1344,8 +1344,8 @@ static u32 atl1_check_link(struct atl1_adapter *adapter)
 
 	/* link result is our setting */
 	if (!reconfig) {
-		if (adapter->link_speed != speed
-		    || adapter->link_duplex != duplex) {
+		if (adapter->link_speed != speed ||
+		    adapter->link_duplex != duplex) {
 			adapter->link_speed = speed;
 			adapter->link_duplex = duplex;
 			atl1_setup_mac_ctrl(adapter);
@@ -1864,21 +1864,14 @@ static u16 atl1_alloc_rx_buffers(struct atl1_adapter *adapter)
 
 		rfd_desc = ATL1_RFD_DESC(rfd_ring, rfd_next_to_use);
 
-		skb = netdev_alloc_skb(adapter->netdev,
-				       adapter->rx_buffer_len + NET_IP_ALIGN);
+		skb = netdev_alloc_skb_ip_align(adapter->netdev,
+						adapter->rx_buffer_len);
 		if (unlikely(!skb)) {
 			/* Better luck next round */
 			adapter->netdev->stats.rx_dropped++;
 			break;
 		}
 
-		/*
-		 * Make buffer alignment 2 beyond a 16 byte boundary
-		 * this will result in a 16 byte aligned IP header after
-		 * the 14 byte MAC header is removed
-		 */
-		skb_reserve(skb, NET_IP_ALIGN);
-
 		buffer_info->alloced = 1;
 		buffer_info->skb = skb;
 		buffer_info->length = (u16) adapter->rx_buffer_len;
@@ -2094,8 +2087,8 @@ static void atl1_intr_tx(struct atl1_adapter *adapter)
 	}
 	atomic_set(&tpd_ring->next_to_clean, sw_tpd_next_to_clean);
 
-	if (netif_queue_stopped(adapter->netdev)
-	    && netif_carrier_ok(adapter->netdev))
+	if (netif_queue_stopped(adapter->netdev) &&
+	    netif_carrier_ok(adapter->netdev))
 		netif_wake_queue(adapter->netdev);
 }
 
@@ -2596,7 +2589,7 @@ static s32 atl1_up(struct atl1_adapter *adapter)
 		irq_flags |= IRQF_SHARED;
 	}
 
-	err = request_irq(adapter->pdev->irq, &atl1_intr, irq_flags,
+	err = request_irq(adapter->pdev->irq, atl1_intr, irq_flags,
 			netdev->name, netdev);
 	if (unlikely(err))
 		goto err_up;
diff --git a/drivers/net/atlx/atl2.c b/drivers/net/atlx/atl2.c
index ab688862093f..c0451d75cdcf 100644
--- a/drivers/net/atlx/atl2.c
+++ b/drivers/net/atlx/atl2.c
@@ -409,7 +409,7 @@ static void atl2_intr_rx(struct atl2_adapter *adapter)
 		if (rxd->status.ok && rxd->status.pkt_size >= 60) {
 			int rx_size = (int)(rxd->status.pkt_size - 4);
 			/* alloc new buffer */
-			skb = netdev_alloc_skb(netdev, rx_size + NET_IP_ALIGN);
+			skb = netdev_alloc_skb_ip_align(netdev, rx_size);
 			if (NULL == skb) {
 				printk(KERN_WARNING
 					"%s: Mem squeeze, deferring packet.\n",
@@ -421,7 +421,6 @@ static void atl2_intr_rx(struct atl2_adapter *adapter)
 				netdev->stats.rx_dropped++;
 				break;
 			}
-			skb_reserve(skb, NET_IP_ALIGN);
 			skb->dev = netdev;
 			memcpy(skb->data, rxd->packet, rx_size);
 			skb_put(skb, rx_size);
@@ -652,7 +651,7 @@ static int atl2_request_irq(struct atl2_adapter *adapter)
 	if (adapter->have_msi)
 		flags &= ~IRQF_SHARED;
 
-	return request_irq(adapter->pdev->irq, &atl2_intr, flags, netdev->name,
+	return request_irq(adapter->pdev->irq, atl2_intr, flags, netdev->name,
 		netdev);
 }
 
diff --git a/drivers/net/atp.c b/drivers/net/atp.c
index 9043294fe617..2f8261c9614a 100644
--- a/drivers/net/atp.c
+++ b/drivers/net/atp.c
@@ -437,7 +437,7 @@ static int net_open(struct net_device *dev)
 	/* The interrupt line is turned off (tri-stated) when the device isn't in
 	   use.  That's especially important for "attached" interfaces where the
 	   port or interrupt may be shared. */
-	ret = request_irq(dev->irq, &atp_interrupt, 0, dev->name, dev);
+	ret = request_irq(dev->irq, atp_interrupt, 0, dev->name, dev);
 	if (ret)
 		return ret;
 
@@ -673,8 +673,8 @@ static irqreturn_t atp_interrupt(int irq, void *dev_instance)
 				netif_wake_queue(dev);	/* Inform upper layers. */
 			}
 			num_tx_since_rx++;
-		} else if (num_tx_since_rx > 8
-				   && time_after(jiffies, dev->last_rx + HZ)) {
+		} else if (num_tx_since_rx > 8 &&
+			   time_after(jiffies, dev->last_rx + HZ)) {
 			if (net_debug > 2)
 				printk(KERN_DEBUG "%s: Missed packet? No Rx after %d Tx and "
 					   "%ld jiffies status %02x  CMR1 %02x.\n", dev->name,
diff --git a/drivers/net/au1000_eth.c b/drivers/net/au1000_eth.c
index 3f4b4300f533..6bac04603a88 100644
--- a/drivers/net/au1000_eth.c
+++ b/drivers/net/au1000_eth.c
@@ -881,7 +881,7 @@ static int au1000_open(struct net_device *dev)
 	if (au1000_debug > 4)
 		printk("%s: open: dev=%p\n", dev->name, dev);
 
-	if ((retval = request_irq(dev->irq, &au1000_interrupt, 0,
+	if ((retval = request_irq(dev->irq, au1000_interrupt, 0,
 					dev->name, dev))) {
 		printk(KERN_ERR "%s: unable to get IRQ %d\n",
 				dev->name, dev->irq);
diff --git a/drivers/net/bcm63xx_enet.c b/drivers/net/bcm63xx_enet.c
index ba29dc319b34..1f6c5486d715 100644
--- a/drivers/net/bcm63xx_enet.c
+++ b/drivers/net/bcm63xx_enet.c
@@ -320,16 +320,13 @@ static int bcm_enet_receive_queue(struct net_device *dev, int budget)
 		if (len < copybreak) {
 			struct sk_buff *nskb;
 
-			nskb = netdev_alloc_skb(dev, len + NET_IP_ALIGN);
+			nskb = netdev_alloc_skb_ip_align(dev, len);
 			if (!nskb) {
 				/* forget packet, just rearm desc */
 				priv->stats.rx_dropped++;
 				continue;
 			}
 
-			/* since we're copying the data, we can align
-			 * them properly */
-			skb_reserve(nskb, NET_IP_ALIGN);
 			dma_sync_single_for_cpu(kdev, desc->address,
 						len, DMA_FROM_DEVICE);
 			memcpy(nskb->data, skb->data, len);
diff --git a/drivers/net/benet/be.h b/drivers/net/benet/be.h
index 3b79a225628a..9e56014d27ed 100644
--- a/drivers/net/benet/be.h
+++ b/drivers/net/benet/be.h
@@ -32,23 +32,34 @@
 
 #include "be_hw.h"
 
-#define DRV_VER			"2.101.205"
+#define DRV_VER			"2.101.346u"
 #define DRV_NAME		"be2net"
 #define BE_NAME			"ServerEngines BladeEngine2 10Gbps NIC"
+#define BE3_NAME		"ServerEngines BladeEngine3 10Gbps NIC"
 #define OC_NAME			"Emulex OneConnect 10Gbps NIC"
+#define OC_NAME1		"Emulex OneConnect 10Gbps NIC (be3)"
 #define DRV_DESC		BE_NAME "Driver"
 
 #define BE_VENDOR_ID 		0x19a2
 #define BE_DEVICE_ID1		0x211
+#define BE_DEVICE_ID2		0x221
 #define OC_DEVICE_ID1		0x700
 #define OC_DEVICE_ID2		0x701
+#define OC_DEVICE_ID3		0x710
 
 static inline char *nic_name(struct pci_dev *pdev)
 {
-	if (pdev->device == OC_DEVICE_ID1 || pdev->device == OC_DEVICE_ID2)
+	switch (pdev->device) {
+	case OC_DEVICE_ID1:
+	case OC_DEVICE_ID2:
 		return OC_NAME;
-	else
+	case OC_DEVICE_ID3:
+		return OC_NAME1;
+	case BE_DEVICE_ID2:
+		return BE3_NAME;
+	default:
 		return BE_NAME;
+	}
 }
 
 /* Number of bytes of an RX frame that are copied to skb->data */
@@ -159,7 +170,7 @@ struct be_drvr_stats {
 	u32 cache_barrier[16];
 
 	u32 be_ethrx_post_fail;/* number of ethrx buffer alloc failures */
-	u32 be_polls;		/* number of times NAPI called poll function */
+	u32 be_rx_polls;	/* number of times NAPI called poll function */
 	u32 be_rx_events;	/* number of ucast rx completion events  */
 	u32 be_rx_compl;	/* number of rx completion entries processed */
 	ulong be_rx_jiffies;
@@ -181,7 +192,6 @@ struct be_drvr_stats {
 
 struct be_stats_obj {
 	struct be_drvr_stats drvr_stats;
-	struct net_device_stats net_stats;
 	struct be_dma_mem cmd;
 };
 
@@ -244,6 +254,7 @@ struct be_adapter {
 	struct vlan_group *vlan_grp;
 	u16 num_vlans;
 	u8 vlan_tag[VLAN_GROUP_ARRAY_LEN];
+	struct be_dma_mem mc_cmd_mem;
 
 	struct be_stats_obj stats;
 	/* Work queue used to perform periodic tasks like getting statistics */
@@ -258,9 +269,12 @@ struct be_adapter {
 	bool link_up;
 	u32 port_num;
 	bool promiscuous;
+	bool wol;
 	u32 cap;
 	u32 rx_fc;		/* Rx flow control */
 	u32 tx_fc;		/* Tx flow control */
+	int link_speed;
+	u8 port_type;
 };
 
 extern const struct ethtool_ops be_ethtool_ops;
diff --git a/drivers/net/benet/be_cmds.c b/drivers/net/benet/be_cmds.c
index 28a0eda92680..1b68bd98dc0c 100644
--- a/drivers/net/benet/be_cmds.c
+++ b/drivers/net/benet/be_cmds.c
@@ -71,8 +71,8 @@ static int be_mcc_compl_process(struct be_adapter *adapter,
 		extd_status = (compl->status >> CQE_STATUS_EXTD_SHIFT) &
 				CQE_STATUS_EXTD_MASK;
 		dev_warn(&adapter->pdev->dev,
-			"Error in cmd completion: status(compl/extd)=%d/%d\n",
-			compl_status, extd_status);
+		"Error in cmd completion - opcode %d, compl %d, extd %d\n",
+			compl->tag0, compl_status, extd_status);
 	}
 	return compl_status;
 }
@@ -277,7 +277,7 @@ static inline struct be_sge *nonembedded_sgl(struct be_mcc_wrb *wrb)
 
 /* Don't touch the hdr after it's prepared */
 static void be_wrb_hdr_prepare(struct be_mcc_wrb *wrb, int payload_len,
-				bool embedded, u8 sge_cnt)
+				bool embedded, u8 sge_cnt, u32 opcode)
 {
 	if (embedded)
 		wrb->embedded |= MCC_WRB_EMBEDDED_MASK;
@@ -285,6 +285,7 @@ static void be_wrb_hdr_prepare(struct be_mcc_wrb *wrb, int payload_len,
 		wrb->embedded |= (sge_cnt & MCC_WRB_SGE_CNT_MASK) <<
 				MCC_WRB_SGE_CNT_SHIFT;
 	wrb->payload_length = payload_len;
+	wrb->tag0 = opcode;
 	be_dws_cpu_to_le(wrb, 20);
 }
 
@@ -349,7 +350,11 @@ static struct be_mcc_wrb *wrb_from_mccq(struct be_adapter *adapter)
 	struct be_queue_info *mccq = &adapter->mcc_obj.q;
 	struct be_mcc_wrb *wrb;
 
-	BUG_ON(atomic_read(&mccq->used) >= mccq->len);
+	if (atomic_read(&mccq->used) >= mccq->len) {
+		dev_err(&adapter->pdev->dev, "Out of MCCQ wrbs\n");
+		return NULL;
+	}
+
 	wrb = queue_head_node(mccq);
 	queue_head_inc(mccq);
 	atomic_inc(&mccq->used);
@@ -357,6 +362,57 @@ static struct be_mcc_wrb *wrb_from_mccq(struct be_adapter *adapter)
 	return wrb;
 }
 
+/* Tell fw we're about to start firing cmds by writing a
+ * special pattern across the wrb hdr; uses mbox
+ */
+int be_cmd_fw_init(struct be_adapter *adapter)
+{
+	u8 *wrb;
+	int status;
+
+	spin_lock(&adapter->mbox_lock);
+
+	wrb = (u8 *)wrb_from_mbox(adapter);
+	*wrb++ = 0xFF;
+	*wrb++ = 0x12;
+	*wrb++ = 0x34;
+	*wrb++ = 0xFF;
+	*wrb++ = 0xFF;
+	*wrb++ = 0x56;
+	*wrb++ = 0x78;
+	*wrb = 0xFF;
+
+	status = be_mbox_notify_wait(adapter);
+
+	spin_unlock(&adapter->mbox_lock);
+	return status;
+}
+
+/* Tell fw we're done with firing cmds by writing a
+ * special pattern across the wrb hdr; uses mbox
+ */
+int be_cmd_fw_clean(struct be_adapter *adapter)
+{
+	u8 *wrb;
+	int status;
+
+	spin_lock(&adapter->mbox_lock);
+
+	wrb = (u8 *)wrb_from_mbox(adapter);
+	*wrb++ = 0xFF;
+	*wrb++ = 0xAA;
+	*wrb++ = 0xBB;
+	*wrb++ = 0xFF;
+	*wrb++ = 0xFF;
+	*wrb++ = 0xCC;
+	*wrb++ = 0xDD;
+	*wrb = 0xFF;
+
+	status = be_mbox_notify_wait(adapter);
+
+	spin_unlock(&adapter->mbox_lock);
+	return status;
+}
 int be_cmd_eq_create(struct be_adapter *adapter,
 		struct be_queue_info *eq, int eq_delay)
 {
@@ -370,7 +426,7 @@ int be_cmd_eq_create(struct be_adapter *adapter,
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0, OPCODE_COMMON_EQ_CREATE);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_EQ_CREATE, sizeof(*req));
@@ -414,7 +470,8 @@ int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_NTWK_MAC_QUERY);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_MAC_QUERY, sizeof(*req));
@@ -448,9 +505,14 @@ int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_NTWK_PMAC_ADD);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_PMAC_ADD, sizeof(*req));
@@ -464,6 +526,7 @@ int be_cmd_pmac_add(struct be_adapter *adapter, u8 *mac_addr,
 		*pmac_id = le32_to_cpu(resp->pmac_id);
 	}
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
@@ -478,9 +541,14 @@ int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, u32 pmac_id)
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_NTWK_PMAC_DEL);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_PMAC_DEL, sizeof(*req));
@@ -490,8 +558,8 @@ int be_cmd_pmac_del(struct be_adapter *adapter, u32 if_id, u32 pmac_id)
 
 	status = be_mcc_notify_wait(adapter);
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
-
 	return status;
 }
 
@@ -512,7 +580,8 @@ int be_cmd_cq_create(struct be_adapter *adapter,
 	req = embedded_payload(wrb);
 	ctxt = &req->context;
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_CQ_CREATE);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_CQ_CREATE, sizeof(*req));
@@ -569,7 +638,8 @@ int be_cmd_mccq_create(struct be_adapter *adapter,
 	req = embedded_payload(wrb);
 	ctxt = &req->context;
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_MCC_CREATE);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 			OPCODE_COMMON_MCC_CREATE, sizeof(*req));
@@ -613,7 +683,8 @@ int be_cmd_txq_create(struct be_adapter *adapter,
 	req = embedded_payload(wrb);
 	ctxt = &req->context;
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_ETH_TX_CREATE);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ETH, OPCODE_ETH_TX_CREATE,
 		sizeof(*req));
@@ -660,7 +731,8 @@ int be_cmd_rxq_create(struct be_adapter *adapter,
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_ETH_RX_CREATE);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ETH, OPCODE_ETH_RX_CREATE,
 		sizeof(*req));
@@ -701,8 +773,6 @@ int be_cmd_q_destroy(struct be_adapter *adapter, struct be_queue_info *q,
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
-
 	switch (queue_type) {
 	case QTYPE_EQ:
 		subsys = CMD_SUBSYSTEM_COMMON;
@@ -727,6 +797,9 @@ int be_cmd_q_destroy(struct be_adapter *adapter, struct be_queue_info *q,
 	default:
 		BUG();
 	}
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0, opcode);
+
 	be_cmd_hdr_prepare(&req->hdr, subsys, opcode, sizeof(*req));
 	req->id = cpu_to_le16(q->id);
 
@@ -752,7 +825,8 @@ int be_cmd_if_create(struct be_adapter *adapter, u32 cap_flags, u32 en_flags,
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_NTWK_INTERFACE_CREATE);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_INTERFACE_CREATE, sizeof(*req));
@@ -787,7 +861,8 @@ int be_cmd_if_destroy(struct be_adapter *adapter, u32 interface_id)
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_NTWK_INTERFACE_DESTROY);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_INTERFACE_DESTROY, sizeof(*req));
@@ -810,15 +885,20 @@ int be_cmd_get_stats(struct be_adapter *adapter, struct be_dma_mem *nonemb_cmd)
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_get_stats *req;
 	struct be_sge *sge;
+	int status = 0;
 
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = nonemb_cmd->va;
 	sge = nonembedded_sgl(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), false, 1);
-	wrb->tag0 = OPCODE_ETH_GET_STATISTICS;
+	be_wrb_hdr_prepare(wrb, sizeof(*req), false, 1,
+			OPCODE_ETH_GET_STATISTICS);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ETH,
 		OPCODE_ETH_GET_STATISTICS, sizeof(*req));
@@ -828,13 +908,14 @@ int be_cmd_get_stats(struct be_adapter *adapter, struct be_dma_mem *nonemb_cmd)
 
 	be_mcc_notify(adapter);
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
-	return 0;
+	return status;
 }
 
 /* Uses synchronous mcc */
 int be_cmd_link_status_query(struct be_adapter *adapter,
-			bool *link_up)
+			bool *link_up, u8 *mac_speed, u16 *link_speed)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_link_status *req;
@@ -843,11 +924,16 @@ int be_cmd_link_status_query(struct be_adapter *adapter,
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
 	*link_up = false;
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_NTWK_LINK_STATUS_QUERY);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_LINK_STATUS_QUERY, sizeof(*req));
@@ -855,10 +941,14 @@ int be_cmd_link_status_query(struct be_adapter *adapter,
 	status = be_mcc_notify_wait(adapter);
 	if (!status) {
 		struct be_cmd_resp_link_status *resp = embedded_payload(wrb);
-		if (resp->mac_speed != PHY_LINK_SPEED_ZERO)
+		if (resp->mac_speed != PHY_LINK_SPEED_ZERO) {
 			*link_up = true;
+			*link_speed = le16_to_cpu(resp->link_speed);
+			*mac_speed = resp->mac_speed;
+		}
 	}
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
@@ -875,7 +965,8 @@ int be_cmd_get_fw_ver(struct be_adapter *adapter, char *fw_ver)
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_GET_FW_VERSION);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_GET_FW_VERSION, sizeof(*req));
@@ -897,13 +988,19 @@ int be_cmd_modify_eqd(struct be_adapter *adapter, u32 eq_id, u32 eqd)
 {
 	struct be_mcc_wrb *wrb;
 	struct be_cmd_req_modify_eq_delay *req;
+	int status = 0;
 
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_MODIFY_EQ_DELAY);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_MODIFY_EQ_DELAY, sizeof(*req));
@@ -915,8 +1012,9 @@ int be_cmd_modify_eqd(struct be_adapter *adapter, u32 eq_id, u32 eqd)
 
 	be_mcc_notify(adapter);
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
-	return 0;
+	return status;
 }
 
 /* Uses sycnhronous mcc */
@@ -930,9 +1028,14 @@ int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_NTWK_VLAN_CONFIG);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_VLAN_CONFIG, sizeof(*req));
@@ -948,6 +1051,7 @@ int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id, u16 *vtag_array,
 
 	status = be_mcc_notify_wait(adapter);
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
@@ -964,9 +1068,13 @@ int be_cmd_promiscuous_config(struct be_adapter *adapter, u8 port_num, bool en)
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0, OPCODE_ETH_PROMISCUOUS);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ETH,
 		OPCODE_ETH_PROMISCUOUS, sizeof(*req));
@@ -978,6 +1086,7 @@ int be_cmd_promiscuous_config(struct be_adapter *adapter, u8 port_num, bool en)
 
 	status = be_mcc_notify_wait(adapter);
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
@@ -987,24 +1096,35 @@ int be_cmd_promiscuous_config(struct be_adapter *adapter, u8 port_num, bool en)
  * (mc == NULL) => multicast promiscous
  */
 int be_cmd_multicast_set(struct be_adapter *adapter, u32 if_id,
-		struct dev_mc_list *mc_list, u32 mc_count)
+		struct dev_mc_list *mc_list, u32 mc_count,
+		struct be_dma_mem *mem)
 {
-#define BE_MAX_MC		32 /* set mcast promisc if > 32 */
 	struct be_mcc_wrb *wrb;
-	struct be_cmd_req_mcast_mac_config *req;
+	struct be_cmd_req_mcast_mac_config *req = mem->va;
+	struct be_sge *sge;
+	int status;
 
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
-	req = embedded_payload(wrb);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	sge = nonembedded_sgl(wrb);
+	memset(req, 0, sizeof(*req));
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), false, 1,
+			OPCODE_COMMON_NTWK_MULTICAST_SET);
+	sge->pa_hi = cpu_to_le32(upper_32_bits(mem->dma));
+	sge->pa_lo = cpu_to_le32(mem->dma & 0xFFFFFFFF);
+	sge->len = cpu_to_le32(mem->size);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_NTWK_MULTICAST_SET, sizeof(*req));
 
 	req->interface_id = if_id;
-	if (mc_list && mc_count <= BE_MAX_MC) {
+	if (mc_list) {
 		int i;
 		struct dev_mc_list *mc;
 
@@ -1016,11 +1136,11 @@ int be_cmd_multicast_set(struct be_adapter *adapter, u32 if_id,
 		req->promiscuous = 1;
 	}
 
-	be_mcc_notify_wait(adapter);
+	status = be_mcc_notify_wait(adapter);
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
-
-	return 0;
+	return status;
 }
 
 /* Uses synchrounous mcc */
@@ -1033,9 +1153,14 @@ int be_cmd_set_flow_control(struct be_adapter *adapter, u32 tx_fc, u32 rx_fc)
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_SET_FLOW_CONTROL);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_SET_FLOW_CONTROL, sizeof(*req));
@@ -1045,6 +1170,7 @@ int be_cmd_set_flow_control(struct be_adapter *adapter, u32 tx_fc, u32 rx_fc)
 
 	status = be_mcc_notify_wait(adapter);
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
@@ -1059,9 +1185,14 @@ int be_cmd_get_flow_control(struct be_adapter *adapter, u32 *tx_fc, u32 *rx_fc)
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_GET_FLOW_CONTROL);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_GET_FLOW_CONTROL, sizeof(*req));
@@ -1074,6 +1205,7 @@ int be_cmd_get_flow_control(struct be_adapter *adapter, u32 *tx_fc, u32 *rx_fc)
 		*rx_fc = le16_to_cpu(resp->rx_flow_control);
 	}
 
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
@@ -1090,7 +1222,8 @@ int be_cmd_query_fw_cfg(struct be_adapter *adapter, u32 *port_num, u32 *cap)
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_QUERY_FIRMWARE_CONFIG);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_QUERY_FIRMWARE_CONFIG, sizeof(*req));
@@ -1118,7 +1251,8 @@ int be_cmd_reset_function(struct be_adapter *adapter)
 	wrb = wrb_from_mbox(adapter);
 	req = embedded_payload(wrb);
 
-	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0);
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_FUNCTION_RESET);
 
 	be_cmd_hdr_prepare(req, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_FUNCTION_RESET, sizeof(*req));
@@ -1129,6 +1263,113 @@ int be_cmd_reset_function(struct be_adapter *adapter)
 	return status;
 }
 
+/* Uses sync mcc */
+int be_cmd_set_beacon_state(struct be_adapter *adapter, u8 port_num,
+			u8 bcn, u8 sts, u8 state)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_enable_disable_beacon *req;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = embedded_payload(wrb);
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_ENABLE_DISABLE_BEACON);
+
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+		OPCODE_COMMON_ENABLE_DISABLE_BEACON, sizeof(*req));
+
+	req->port_num = port_num;
+	req->beacon_state = state;
+	req->beacon_duration = bcn;
+	req->status_duration = sts;
+
+	status = be_mcc_notify_wait(adapter);
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+/* Uses sync mcc */
+int be_cmd_get_beacon_state(struct be_adapter *adapter, u8 port_num, u32 *state)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_get_beacon_state *req;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = embedded_payload(wrb);
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+			OPCODE_COMMON_GET_BEACON_STATE);
+
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+		OPCODE_COMMON_GET_BEACON_STATE, sizeof(*req));
+
+	req->port_num = port_num;
+
+	status = be_mcc_notify_wait(adapter);
+	if (!status) {
+		struct be_cmd_resp_get_beacon_state *resp =
+						embedded_payload(wrb);
+		*state = resp->beacon_state;
+	}
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+/* Uses sync mcc */
+int be_cmd_read_port_type(struct be_adapter *adapter, u32 port,
+				u8 *connector)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_port_type *req;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = embedded_payload(wrb);
+
+	be_wrb_hdr_prepare(wrb, sizeof(struct be_cmd_resp_port_type), true, 0,
+			OPCODE_COMMON_READ_TRANSRECV_DATA);
+
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+		OPCODE_COMMON_READ_TRANSRECV_DATA, sizeof(*req));
+
+	req->port = cpu_to_le32(port);
+	req->page_num = cpu_to_le32(TR_PAGE_A0);
+	status = be_mcc_notify_wait(adapter);
+	if (!status) {
+		struct be_cmd_resp_port_type *resp = embedded_payload(wrb);
+			*connector = resp->data.connector;
+	}
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
 int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
 			u32 flash_type, u32 flash_opcode, u32 buf_size)
 {
@@ -1140,9 +1381,15 @@ int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
 	spin_lock_bh(&adapter->mcc_lock);
 
 	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = cmd->va;
 	sge = nonembedded_sgl(wrb);
 
-	be_wrb_hdr_prepare(wrb, cmd->size, false, 1);
+	be_wrb_hdr_prepare(wrb, cmd->size, false, 1,
+			OPCODE_COMMON_WRITE_FLASHROM);
 
 	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
 		OPCODE_COMMON_WRITE_FLASHROM, cmd->size);
@@ -1156,6 +1403,171 @@ int be_cmd_write_flashrom(struct be_adapter *adapter, struct be_dma_mem *cmd,
 
 	status = be_mcc_notify_wait(adapter);
 
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_write_flashrom *req;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = embedded_payload(wrb);
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req)+4, true, 0,
+			OPCODE_COMMON_READ_FLASHROM);
+
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_COMMON,
+		OPCODE_COMMON_READ_FLASHROM, sizeof(*req)+4);
+
+	req->params.op_type = cpu_to_le32(FLASHROM_TYPE_REDBOOT);
+	req->params.op_code = cpu_to_le32(FLASHROM_OPER_REPORT);
+	req->params.offset = 0x3FFFC;
+	req->params.data_buf_size = 0x4;
+
+	status = be_mcc_notify_wait(adapter);
+	if (!status)
+		memcpy(flashed_crc, req->params.data_buf, 4);
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+extern int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
+				struct be_dma_mem *nonemb_cmd)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_acpi_wol_magic_config *req;
+	struct be_sge *sge;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = nonemb_cmd->va;
+	sge = nonembedded_sgl(wrb);
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req), false, 1,
+			OPCODE_ETH_ACPI_WOL_MAGIC_CONFIG);
+
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_ETH,
+		OPCODE_ETH_ACPI_WOL_MAGIC_CONFIG, sizeof(*req));
+	memcpy(req->magic_mac, mac, ETH_ALEN);
+
+	sge->pa_hi = cpu_to_le32(upper_32_bits(nonemb_cmd->dma));
+	sge->pa_lo = cpu_to_le32(nonemb_cmd->dma & 0xFFFFFFFF);
+	sge->len = cpu_to_le32(nonemb_cmd->size);
+
+	status = be_mcc_notify_wait(adapter);
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+int be_cmd_loopback_test(struct be_adapter *adapter, u32 port_num,
+		u32 loopback_type, u32 pkt_size, u32 num_pkts, u64 pattern)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_loopback_test *req;
+	int status;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+
+	req = embedded_payload(wrb);
+
+	be_wrb_hdr_prepare(wrb, sizeof(*req), true, 0,
+				OPCODE_LOWLEVEL_LOOPBACK_TEST);
+
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_LOWLEVEL,
+			OPCODE_LOWLEVEL_LOOPBACK_TEST, sizeof(*req));
+
+	req->pattern = cpu_to_le64(pattern);
+	req->src_port = cpu_to_le32(port_num);
+	req->dest_port = cpu_to_le32(port_num);
+	req->pkt_size = cpu_to_le32(pkt_size);
+	req->num_pkts = cpu_to_le32(num_pkts);
+	req->loopback_type = cpu_to_le32(loopback_type);
+
+	status = be_mcc_notify_wait(adapter);
+	if (!status) {
+		struct be_cmd_resp_loopback_test *resp = embedded_payload(wrb);
+		status = le32_to_cpu(resp->status);
+	}
+
+err:
+	spin_unlock_bh(&adapter->mcc_lock);
+	return status;
+}
+
+int be_cmd_ddr_dma_test(struct be_adapter *adapter, u64 pattern,
+				u32 byte_cnt, struct be_dma_mem *cmd)
+{
+	struct be_mcc_wrb *wrb;
+	struct be_cmd_req_ddrdma_test *req;
+	struct be_sge *sge;
+	int status;
+	int i, j = 0;
+
+	spin_lock_bh(&adapter->mcc_lock);
+
+	wrb = wrb_from_mccq(adapter);
+	if (!wrb) {
+		status = -EBUSY;
+		goto err;
+	}
+	req = cmd->va;
+	sge = nonembedded_sgl(wrb);
+	be_wrb_hdr_prepare(wrb, cmd->size, false, 1,
+				OPCODE_LOWLEVEL_HOST_DDR_DMA);
+	be_cmd_hdr_prepare(&req->hdr, CMD_SUBSYSTEM_LOWLEVEL,
+			OPCODE_LOWLEVEL_HOST_DDR_DMA, cmd->size);
+
+	sge->pa_hi = cpu_to_le32(upper_32_bits(cmd->dma));
+	sge->pa_lo = cpu_to_le32(cmd->dma & 0xFFFFFFFF);
+	sge->len = cpu_to_le32(cmd->size);
+
+	req->pattern = cpu_to_le64(pattern);
+	req->byte_count = cpu_to_le32(byte_cnt);
+	for (i = 0; i < byte_cnt; i++) {
+		req->snd_buff[i] = (u8)(pattern >> (j*8));
+		j++;
+		if (j > 7)
+			j = 0;
+	}
+
+	status = be_mcc_notify_wait(adapter);
+
+	if (!status) {
+		struct be_cmd_resp_ddrdma_test *resp;
+		resp = cmd->va;
+		if ((memcmp(resp->rcv_buff, req->snd_buff, byte_cnt) != 0) ||
+				resp->snd_err) {
+			status = -1;
+		}
+	}
+
+err:
 	spin_unlock_bh(&adapter->mcc_lock);
 	return status;
 }
diff --git a/drivers/net/benet/be_cmds.h b/drivers/net/benet/be_cmds.h
index e5f9676cf1bc..e7323be507d0 100644
--- a/drivers/net/benet/be_cmds.h
+++ b/drivers/net/benet/be_cmds.h
@@ -112,12 +112,14 @@ struct be_mcc_mailbox {
 
 #define CMD_SUBSYSTEM_COMMON	0x1
 #define CMD_SUBSYSTEM_ETH 	0x3
+#define CMD_SUBSYSTEM_LOWLEVEL  0xb
 
 #define OPCODE_COMMON_NTWK_MAC_QUERY			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_CQ_CREATE				12
 #define OPCODE_COMMON_EQ_CREATE				13
@@ -138,6 +140,9 @@ struct be_mcc_mailbox {
 #define OPCODE_COMMON_NTWK_PMAC_ADD			59
 #define OPCODE_COMMON_NTWK_PMAC_DEL			60
 #define OPCODE_COMMON_FUNCTION_RESET			61
+#define OPCODE_COMMON_ENABLE_DISABLE_BEACON		69
+#define OPCODE_COMMON_GET_BEACON_STATE			70
+#define OPCODE_COMMON_READ_TRANSRECV_DATA		73
 
 #define OPCODE_ETH_ACPI_CONFIG				2
 #define OPCODE_ETH_PROMISCUOUS				3
@@ -146,6 +151,10 @@ struct be_mcc_mailbox {
 #define OPCODE_ETH_RX_CREATE            		8
 #define OPCODE_ETH_TX_DESTROY           		9
 #define OPCODE_ETH_RX_DESTROY           		10
+#define OPCODE_ETH_ACPI_WOL_MAGIC_CONFIG		12
+
+#define OPCODE_LOWLEVEL_HOST_DDR_DMA                    17
+#define OPCODE_LOWLEVEL_LOOPBACK_TEST                   18
 
 struct be_cmd_req_hdr {
 	u8 opcode;		/* dword 0 */
@@ -587,6 +596,8 @@ struct be_cmd_req_promiscuous_config {
 	u16 rsvd0;
 } __packed;
 
+/******************** Multicast MAC Config *******************/
+#define BE_MAX_MC		64 /* set mcast promisc if > 64 */
 struct macaddr {
 	u8 byte[ETH_ALEN];
 };
@@ -596,7 +607,7 @@ struct be_cmd_req_mcast_mac_config {
 	u16 num_mac;
 	u8 promiscuous;
 	u8 interface_id;
-	struct macaddr mac[32];
+	struct macaddr mac[BE_MAX_MC];
 } __packed;
 
 static inline struct be_hw_stats *
@@ -633,9 +644,47 @@ struct be_cmd_resp_link_status {
 	u8 mac_fault;
 	u8 mgmt_mac_duplex;
 	u8 mgmt_mac_speed;
-	u16 rsvd0;
+	u16 link_speed;
+	u32 rsvd0;
 } __packed;
 
+/******************** Port Identification ***************************/
+/*    Identifies the type of port attached to NIC     */
+struct be_cmd_req_port_type {
+	struct be_cmd_req_hdr hdr;
+	u32 page_num;
+	u32 port;
+};
+
+enum {
+	TR_PAGE_A0 = 0xa0,
+	TR_PAGE_A2 = 0xa2
+};
+
+struct be_cmd_resp_port_type {
+	struct be_cmd_resp_hdr hdr;
+	u32 page_num;
+	u32 port;
+	struct data {
+		u8 identifier;
+		u8 identifier_ext;
+		u8 connector;
+		u8 transceiver[8];
+		u8 rsvd0[3];
+		u8 length_km;
+		u8 length_hm;
+		u8 length_om1;
+		u8 length_om2;
+		u8 length_cu;
+		u8 length_cu_m;
+		u8 vendor_name[16];
+		u8 rsvd;
+		u8 vendor_oui[3];
+		u8 vendor_pn[16];
+		u8 vendor_rev[4];
+	} data;
+};
+
 /******************** Get FW Version *******************/
 struct be_cmd_req_get_fw_version {
 	struct be_cmd_req_hdr hdr;
@@ -699,6 +748,37 @@ struct be_cmd_resp_query_fw_cfg {
 	u32 rsvd[26];
 };
 
+/******************** Port Beacon ***************************/
+
+#define BEACON_STATE_ENABLED		0x1
+#define BEACON_STATE_DISABLED		0x0
+
+struct be_cmd_req_enable_disable_beacon {
+	struct be_cmd_req_hdr hdr;
+	u8  port_num;
+	u8  beacon_state;
+	u8  beacon_duration;
+	u8  status_duration;
+} __packed;
+
+struct be_cmd_resp_enable_disable_beacon {
+	struct be_cmd_resp_hdr resp_hdr;
+	u32 rsvd0;
+} __packed;
+
+struct be_cmd_req_get_beacon_state {
+	struct be_cmd_req_hdr hdr;
+	u8  port_num;
+	u8  rsvd0;
+	u16 rsvd1;
+} __packed;
+
+struct be_cmd_resp_get_beacon_state {
+	struct be_cmd_resp_hdr resp_hdr;
+	u8 beacon_state;
+	u8 rsvd0[3];
+} __packed;
+
 /****************** Firmware Flash ******************/
 struct flashrom_params {
 	u32 op_code;
@@ -713,6 +793,53 @@ struct be_cmd_write_flashrom {
 	struct flashrom_params params;
 };
 
+/************************ WOL *******************************/
+struct be_cmd_req_acpi_wol_magic_config{
+	struct be_cmd_req_hdr hdr;
+	u32 rsvd0[145];
+	u8 magic_mac[6];
+	u8 rsvd2[2];
+} __packed;
+
+/********************** LoopBack test *********************/
+struct be_cmd_req_loopback_test {
+	struct be_cmd_req_hdr hdr;
+	u32 loopback_type;
+	u32 num_pkts;
+	u64 pattern;
+	u32 src_port;
+	u32 dest_port;
+	u32 pkt_size;
+};
+
+struct be_cmd_resp_loopback_test {
+	struct be_cmd_resp_hdr resp_hdr;
+	u32    status;
+	u32    num_txfer;
+	u32    num_rx;
+	u32    miscomp_off;
+	u32    ticks_compl;
+};
+
+/********************** DDR DMA test *********************/
+struct be_cmd_req_ddrdma_test {
+	struct be_cmd_req_hdr hdr;
+	u64 pattern;
+	u32 byte_count;
+	u32 rsvd0;
+	u8  snd_buff[4096];
+	u8  rsvd1[4096];
+};
+
+struct be_cmd_resp_ddrdma_test {
+	struct be_cmd_resp_hdr hdr;
+	u64 pattern;
+	u32 byte_cnt;
+	u32 snd_err;
+	u8  rsvd0[4096];
+	u8  rcv_buff[4096];
+};
+
 extern int be_pci_fnum_get(struct be_adapter *adapter);
 extern int be_cmd_POST(struct be_adapter *adapter);
 extern int be_cmd_mac_addr_query(struct be_adapter *adapter, u8 *mac_addr,
@@ -743,7 +870,7 @@ extern int be_cmd_rxq_create(struct be_adapter *adapter,
 extern int be_cmd_q_destroy(struct be_adapter *adapter, struct be_queue_info *q,
 			int type);
 extern int be_cmd_link_status_query(struct be_adapter *adapter,
-			bool *link_up);
+			bool *link_up, u8 *mac_speed, u16 *link_speed);
 extern int be_cmd_reset(struct be_adapter *adapter);
 extern int be_cmd_get_stats(struct be_adapter *adapter,
 			struct be_dma_mem *nonemb_cmd);
@@ -756,7 +883,8 @@ extern int be_cmd_vlan_config(struct be_adapter *adapter, u32 if_id,
 extern int be_cmd_promiscuous_config(struct be_adapter *adapter,
 			u8 port_num, bool en);
 extern int be_cmd_multicast_set(struct be_adapter *adapter, u32 if_id,
-			struct dev_mc_list *mc_list, u32 mc_count);
+			struct dev_mc_list *mc_list, u32 mc_count,
+			struct be_dma_mem *mem);
 extern int be_cmd_set_flow_control(struct be_adapter *adapter,
 			u32 tx_fc, u32 rx_fc);
 extern int be_cmd_get_flow_control(struct be_adapter *adapter,
@@ -765,6 +893,22 @@ extern int be_cmd_query_fw_cfg(struct be_adapter *adapter,
 			u32 *port_num, u32 *cap);
 extern int be_cmd_reset_function(struct be_adapter *adapter);
 extern int be_process_mcc(struct be_adapter *adapter);
+extern int be_cmd_set_beacon_state(struct be_adapter *adapter,
+			u8 port_num, u8 beacon, u8 status, u8 state);
+extern int be_cmd_get_beacon_state(struct be_adapter *adapter,
+			u8 port_num, u32 *state);
+extern int be_cmd_read_port_type(struct be_adapter *adapter, u32 port,
+					u8 *connector);
 extern int be_cmd_write_flashrom(struct be_adapter *adapter,
 			struct be_dma_mem *cmd, u32 flash_oper,
 			u32 flash_opcode, u32 buf_size);
+extern int be_cmd_get_flash_crc(struct be_adapter *adapter, u8 *flashed_crc);
+extern int be_cmd_enable_magic_wol(struct be_adapter *adapter, u8 *mac,
+				struct be_dma_mem *nonemb_cmd);
+extern int be_cmd_fw_init(struct be_adapter *adapter);
+extern int be_cmd_fw_clean(struct be_adapter *adapter);
+extern int be_cmd_loopback_test(struct be_adapter *adapter, u32 port_num,
+				u32 loopback_type, u32 pkt_size,
+				u32 num_pkts, u64 pattern);
+extern int be_cmd_ddr_dma_test(struct be_adapter *adapter, u64 pattern,
+			u32 byte_cnt, struct be_dma_mem *cmd);
diff --git a/drivers/net/benet/be_ethtool.c b/drivers/net/benet/be_ethtool.c
index f0fd95b43c07..298b92cbd689 100644
--- a/drivers/net/benet/be_ethtool.c
+++ b/drivers/net/benet/be_ethtool.c
@@ -55,7 +55,7 @@ static const struct be_ethtool_stat et_stats[] = {
 	{DRVSTAT_INFO(be_tx_stops)},
 	{DRVSTAT_INFO(be_fwd_reqs)},
 	{DRVSTAT_INFO(be_tx_wrbs)},
-	{DRVSTAT_INFO(be_polls)},
+	{DRVSTAT_INFO(be_rx_polls)},
 	{DRVSTAT_INFO(be_tx_events)},
 	{DRVSTAT_INFO(be_rx_events)},
 	{DRVSTAT_INFO(be_tx_compl)},
@@ -107,6 +107,18 @@ static const struct be_ethtool_stat et_stats[] = {
 };
 #define ETHTOOL_STATS_NUM ARRAY_SIZE(et_stats)
 
+static const char et_self_tests[][ETH_GSTRING_LEN] = {
+	"MAC Loopback test",
+	"PHY Loopback test",
+	"External Loopback test",
+	"DDR DMA test"
+};
+
+#define ETHTOOL_TESTS_NUM ARRAY_SIZE(et_self_tests)
+#define BE_MAC_LOOPBACK 0x0
+#define BE_PHY_LOOPBACK 0x1
+#define BE_ONE_PORT_EXT_LOOPBACK 0x2
+
 static void
 be_get_drvinfo(struct net_device *netdev, struct ethtool_drvinfo *drvinfo)
 {
@@ -234,7 +246,7 @@ be_get_ethtool_stats(struct net_device *netdev,
 	struct be_rxf_stats *rxf_stats = &hw_stats->rxf;
 	struct be_port_rxf_stats *port_stats =
 			&rxf_stats->port[adapter->port_num];
-	struct net_device_stats *net_stats = &adapter->stats.net_stats;
+	struct net_device_stats *net_stats = &netdev->stats;
 	struct be_erx_stats *erx_stats = &hw_stats->erx;
 	void *p = NULL;
 	int i;
@@ -278,19 +290,78 @@ be_get_stat_strings(struct net_device *netdev, uint32_t stringset,
 			data += ETH_GSTRING_LEN;
 		}
 		break;
+	case ETH_SS_TEST:
+		for (i = 0; i < ETHTOOL_TESTS_NUM; i++) {
+			memcpy(data, et_self_tests[i], ETH_GSTRING_LEN);
+			data += ETH_GSTRING_LEN;
+		}
+		break;
 	}
 }
 
-static int be_get_stats_count(struct net_device *netdev)
+static int be_get_sset_count(struct net_device *netdev, int stringset)
 {
-	return ETHTOOL_STATS_NUM;
+	switch (stringset) {
+	case ETH_SS_TEST:
+		return ETHTOOL_TESTS_NUM;
+	case ETH_SS_STATS:
+		return ETHTOOL_STATS_NUM;
+	default:
+		return -EINVAL;
+	}
 }
 
 static int be_get_settings(struct net_device *netdev, struct ethtool_cmd *ecmd)
 {
-	ecmd->speed = SPEED_10000;
+	struct be_adapter *adapter = netdev_priv(netdev);
+	u8 mac_speed = 0, connector = 0;
+	u16 link_speed = 0;
+	bool link_up = false;
+	int status;
+
+	if (adapter->link_speed < 0) {
+		status = be_cmd_link_status_query(adapter, &link_up,
+						&mac_speed, &link_speed);
+
+		/* link_speed is in units of 10 Mbps */
+		if (link_speed) {
+			ecmd->speed = link_speed*10;
+		} else {
+			switch (mac_speed) {
+			case PHY_LINK_SPEED_1GBPS:
+				ecmd->speed = SPEED_1000;
+				break;
+			case PHY_LINK_SPEED_10GBPS:
+				ecmd->speed = SPEED_10000;
+				break;
+			}
+		}
+
+		status = be_cmd_read_port_type(adapter, adapter->port_num,
+						&connector);
+		switch (connector) {
+		case 7:
+			ecmd->port = PORT_FIBRE;
+			break;
+		default:
+			ecmd->port = PORT_TP;
+			break;
+		}
+
+		/* Save for future use */
+		adapter->link_speed = ecmd->speed;
+		adapter->port_type = ecmd->port;
+	} else {
+		ecmd->speed = adapter->link_speed;
+		ecmd->port = adapter->port_type;
+	}
+
 	ecmd->duplex = DUPLEX_FULL;
 	ecmd->autoneg = AUTONEG_DISABLE;
+	ecmd->supported = (SUPPORTED_10000baseT_Full | SUPPORTED_TP);
+	ecmd->phy_address = adapter->port_num;
+	ecmd->transceiver = XCVR_INTERNAL;
+
 	return 0;
 }
 
@@ -335,6 +406,123 @@ be_set_pauseparam(struct net_device *netdev, struct ethtool_pauseparam *ecmd)
 }
 
 static int
+be_phys_id(struct net_device *netdev, u32 data)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+	int status;
+	u32 cur;
+
+	be_cmd_get_beacon_state(adapter, adapter->port_num, &cur);
+
+	if (cur == BEACON_STATE_ENABLED)
+		return 0;
+
+	if (data < 2)
+		data = 2;
+
+	status = be_cmd_set_beacon_state(adapter, adapter->port_num, 0, 0,
+			BEACON_STATE_ENABLED);
+	set_current_state(TASK_INTERRUPTIBLE);
+	schedule_timeout(data*HZ);
+
+	status = be_cmd_set_beacon_state(adapter, adapter->port_num, 0, 0,
+			BEACON_STATE_DISABLED);
+
+	return status;
+}
+
+static void
+be_get_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	wol->supported = WAKE_MAGIC;
+	if (adapter->wol)
+		wol->wolopts = WAKE_MAGIC;
+	else
+		wol->wolopts = 0;
+	memset(&wol->sopass, 0, sizeof(wol->sopass));
+	return;
+}
+
+static int
+be_set_wol(struct net_device *netdev, struct ethtool_wolinfo *wol)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	if (wol->wolopts & ~WAKE_MAGIC)
+		return -EINVAL;
+
+	if (wol->wolopts & WAKE_MAGIC)
+		adapter->wol = true;
+	else
+		adapter->wol = false;
+
+	return 0;
+}
+
+static int
+be_test_ddr_dma(struct be_adapter *adapter)
+{
+	int ret, i;
+	struct be_dma_mem ddrdma_cmd;
+	u64 pattern[2] = {0x5a5a5a5a5a5a5a5a, 0xa5a5a5a5a5a5a5a5};
+
+	ddrdma_cmd.size = sizeof(struct be_cmd_req_ddrdma_test);
+	ddrdma_cmd.va = pci_alloc_consistent(adapter->pdev, ddrdma_cmd.size,
+					&ddrdma_cmd.dma);
+	if (!ddrdma_cmd.va) {
+		dev_err(&adapter->pdev->dev, "Memory allocation failure \n");
+		return -ENOMEM;
+	}
+
+	for (i = 0; i < 2; i++) {
+		ret = be_cmd_ddr_dma_test(adapter, pattern[i],
+					4096, &ddrdma_cmd);
+		if (ret != 0)
+			goto err;
+	}
+
+err:
+	pci_free_consistent(adapter->pdev, ddrdma_cmd.size,
+			ddrdma_cmd.va, ddrdma_cmd.dma);
+	return ret;
+}
+
+static void
+be_self_test(struct net_device *netdev, struct ethtool_test *test, u64 *data)
+{
+	struct be_adapter *adapter = netdev_priv(netdev);
+
+	memset(data, 0, sizeof(u64) * ETHTOOL_TESTS_NUM);
+
+	if (test->flags & ETH_TEST_FL_OFFLINE) {
+		data[0] = be_cmd_loopback_test(adapter, adapter->port_num,
+						BE_MAC_LOOPBACK, 1500,
+						2, 0xabc);
+		if (data[0] != 0)
+			test->flags |= ETH_TEST_FL_FAILED;
+
+		data[1] = be_cmd_loopback_test(adapter, adapter->port_num,
+						BE_PHY_LOOPBACK, 1500,
+						2, 0xabc);
+		if (data[1] != 0)
+			test->flags |= ETH_TEST_FL_FAILED;
+
+		data[2] = be_cmd_loopback_test(adapter, adapter->port_num,
+						BE_ONE_PORT_EXT_LOOPBACK,
+						1500, 2, 0xabc);
+		if (data[2] != 0)
+			test->flags |= ETH_TEST_FL_FAILED;
+
+		data[3] = be_test_ddr_dma(adapter);
+		if (data[3] != 0)
+			test->flags |= ETH_TEST_FL_FAILED;
+	}
+
+}
+
+static int
 be_do_flash(struct net_device *netdev, struct ethtool_flash *efl)
 {
 	struct be_adapter *adapter = netdev_priv(netdev);
@@ -351,6 +539,8 @@ be_do_flash(struct net_device *netdev, struct ethtool_flash *efl)
 const struct ethtool_ops be_ethtool_ops = {
 	.get_settings = be_get_settings,
 	.get_drvinfo = be_get_drvinfo,
+	.get_wol = be_get_wol,
+	.set_wol = be_set_wol,
 	.get_link = ethtool_op_get_link,
 	.get_coalesce = be_get_coalesce,
 	.set_coalesce = be_set_coalesce,
@@ -366,7 +556,9 @@ const struct ethtool_ops be_ethtool_ops = {
 	.get_tso = ethtool_op_get_tso,
 	.set_tso = ethtool_op_set_tso,
 	.get_strings = be_get_stat_strings,
-	.get_stats_count = be_get_stats_count,
+	.phys_id = be_phys_id,
+	.get_sset_count = be_get_sset_count,
 	.get_ethtool_stats = be_get_ethtool_stats,
 	.flash_device = be_do_flash,
+	.self_test = be_self_test,
 };
diff --git a/drivers/net/benet/be_hw.h b/drivers/net/benet/be_hw.h
index a3394b4aa14a..e2b3beffd49d 100644
--- a/drivers/net/benet/be_hw.h
+++ b/drivers/net/benet/be_hw.h
@@ -52,6 +52,10 @@
  */
 #define MEMBAR_CTRL_INT_CTRL_HOSTINTR_MASK	(1 << 29) /* bit 29 */
 
+/********* Power managment (WOL) **********/
+#define PCICFG_PM_CONTROL_OFFSET		0x44
+#define PCICFG_PM_CONTROL_MASK			0x108	/* bits 3 & 8 */
+
 /********* ISR0 Register offset **********/
 #define CEV_ISR0_OFFSET 			0xC18
 #define CEV_ISR_SIZE				4
@@ -225,6 +229,7 @@ struct be_eth_rx_compl {
 #define NUM_FLASHDIR_ENTRIES		32
 
 #define FLASHROM_TYPE_ISCSI_ACTIVE	0
+#define FLASHROM_TYPE_REDBOOT		1
 #define FLASHROM_TYPE_BIOS		2
 #define FLASHROM_TYPE_PXE_BIOS		3
 #define FLASHROM_TYPE_FCOE_BIOS		8
@@ -234,9 +239,11 @@ struct be_eth_rx_compl {
 
 #define FLASHROM_OPER_FLASH		1
 #define FLASHROM_OPER_SAVE		2
+#define FLASHROM_OPER_REPORT		4
 
 #define FLASH_IMAGE_MAX_SIZE            (1310720) /* Max firmware image size */
 #define FLASH_BIOS_IMAGE_MAX_SIZE       (262144)  /* Max OPTION ROM image sz */
+#define FLASH_REDBOOT_IMAGE_MAX_SIZE    (262144)  /* Max redboot image sz */
 
 /* Offsets for components on Flash. */
 #define FLASH_iSCSI_PRIMARY_IMAGE_START (1048576)
@@ -246,6 +253,8 @@ struct be_eth_rx_compl {
 #define FLASH_iSCSI_BIOS_START          (7340032)
 #define FLASH_PXE_BIOS_START            (7864320)
 #define FLASH_FCoE_BIOS_START           (524288)
+#define FLASH_REDBOOT_START		(32768)
+#define FLASH_REDBOOT_ISM_START		(0)
 
 struct controller_id {
 	u32 vendor;
diff --git a/drivers/net/benet/be_main.c b/drivers/net/benet/be_main.c
index 876b357101fa..957a0f7f2764 100644
--- a/drivers/net/benet/be_main.c
+++ b/drivers/net/benet/be_main.c
@@ -31,8 +31,10 @@ MODULE_PARM_DESC(rx_frag_size, "Size of a fragment that holds rcvd data.");
 
 static DEFINE_PCI_DEVICE_TABLE(be_dev_ids) = {
 	{ PCI_DEVICE(BE_VENDOR_ID, BE_DEVICE_ID1) },
+	{ PCI_DEVICE(BE_VENDOR_ID, BE_DEVICE_ID2) },
 	{ PCI_DEVICE(BE_VENDOR_ID, OC_DEVICE_ID1) },
 	{ PCI_DEVICE(BE_VENDOR_ID, OC_DEVICE_ID2) },
+	{ PCI_DEVICE(BE_VENDOR_ID, OC_DEVICE_ID3) },
 	{ 0 }
 };
 MODULE_DEVICE_TABLE(pci, be_dev_ids);
@@ -123,6 +125,9 @@ static int be_mac_addr_set(struct net_device *netdev, void *p)
 	struct sockaddr *addr = p;
 	int status = 0;
 
+	if (!is_valid_ether_addr(addr->sa_data))
+		return -EADDRNOTAVAIL;
+
 	status = be_cmd_pmac_del(adapter, adapter->if_handle, adapter->pmac_id);
 	if (status)
 		return status;
@@ -141,7 +146,7 @@ void netdev_stats_update(struct be_adapter *adapter)
 	struct be_rxf_stats *rxf_stats = &hw_stats->rxf;
 	struct be_port_rxf_stats *port_stats =
 			&rxf_stats->port[adapter->port_num];
-	struct net_device_stats *dev_stats = &adapter->stats.net_stats;
+	struct net_device_stats *dev_stats = &adapter->netdev->stats;
 	struct be_erx_stats *erx_stats = &hw_stats->erx;
 
 	dev_stats->rx_packets = port_stats->rx_total_frames;
@@ -168,7 +173,8 @@ void netdev_stats_update(struct be_adapter *adapter)
 		port_stats->rx_udp_checksum_errs;
 
 	/*  no space in linux buffers: best possible approximation */
-	dev_stats->rx_dropped = erx_stats->rx_drops_no_fragments[0];
+	dev_stats->rx_dropped =
+		erx_stats->rx_drops_no_fragments[adapter->rx_obj.q.id];
 
 	/* detailed rx errors */
 	dev_stats->rx_length_errors = port_stats->rx_in_range_errors +
@@ -214,6 +220,7 @@ void be_link_status_update(struct be_adapter *adapter, bool link_up)
 
 	/* If link came up or went down */
 	if (adapter->link_up != link_up) {
+		adapter->link_speed = -1;
 		if (link_up) {
 			netif_start_queue(netdev);
 			netif_carrier_on(netdev);
@@ -269,9 +276,7 @@ static void be_rx_eqd_update(struct be_adapter *adapter)
 
 static struct net_device_stats *be_get_stats(struct net_device *dev)
 {
-	struct be_adapter *adapter = netdev_priv(dev);
-
-	return &adapter->stats.net_stats;
+	return &dev->stats;
 }
 
 static u32 be_calc_rate(u64 bytes, unsigned long ticks)
@@ -389,15 +394,11 @@ static int make_tx_wrbs(struct be_adapter *adapter,
 	atomic_add(wrb_cnt, &txq->used);
 	queue_head_inc(txq);
 
-	if (skb_dma_map(&pdev->dev, skb, DMA_TO_DEVICE)) {
-		dev_err(&pdev->dev, "TX DMA mapping failed\n");
-		return 0;
-	}
-
 	if (skb->len > skb->data_len) {
 		int len = skb->len - skb->data_len;
+		busaddr = pci_map_single(pdev, skb->data, len,
+					 PCI_DMA_TODEVICE);
 		wrb = queue_head_node(txq);
-		busaddr = skb_shinfo(skb)->dma_head;
 		wrb_fill(wrb, busaddr, len);
 		be_dws_cpu_to_le(wrb, sizeof(*wrb));
 		queue_head_inc(txq);
@@ -407,8 +408,9 @@ static int make_tx_wrbs(struct be_adapter *adapter,
 	for (i = 0; i < skb_shinfo(skb)->nr_frags; i++) {
 		struct skb_frag_struct *frag =
 			&skb_shinfo(skb)->frags[i];
-
-		busaddr = skb_shinfo(skb)->dma_maps[i];
+		busaddr = pci_map_page(pdev, frag->page,
+				       frag->page_offset,
+				       frag->size, PCI_DMA_TODEVICE);
 		wrb = queue_head_node(txq);
 		wrb_fill(wrb, busaddr, frag->size);
 		be_dws_cpu_to_le(wrb, sizeof(*wrb));
@@ -562,13 +564,15 @@ static void be_set_multicast_list(struct net_device *netdev)
 		be_cmd_promiscuous_config(adapter, adapter->port_num, 0);
 	}
 
-	if (netdev->flags & IFF_ALLMULTI) {
-		be_cmd_multicast_set(adapter, adapter->if_handle, NULL, 0);
+	/* Enable multicast promisc if num configured exceeds what we support */
+	if (netdev->flags & IFF_ALLMULTI || netdev->mc_count > BE_MAX_MC) {
+		be_cmd_multicast_set(adapter, adapter->if_handle, NULL, 0,
+				&adapter->mc_cmd_mem);
 		goto done;
 	}
 
 	be_cmd_multicast_set(adapter, adapter->if_handle, netdev->mc_list,
-		netdev->mc_count);
+		netdev->mc_count, &adapter->mc_cmd_mem);
 done:
 	return;
 }
@@ -758,7 +762,7 @@ static void be_rx_compl_process(struct be_adapter *adapter,
 	if ((adapter->cap == 0x400) && !vtm)
 		vlanf = 0;
 
-	skb = netdev_alloc_skb(adapter->netdev, BE_HDR_LEN + NET_IP_ALIGN);
+	skb = netdev_alloc_skb_ip_align(adapter->netdev, BE_HDR_LEN);
 	if (!skb) {
 		if (net_ratelimit())
 			dev_warn(&adapter->pdev->dev, "skb alloc failed\n");
@@ -766,8 +770,6 @@ static void be_rx_compl_process(struct be_adapter *adapter,
 		return;
 	}
 
-	skb_reserve(skb, NET_IP_ALIGN);
-
 	skb_fill_rx_data(adapter, skb, rxcp);
 
 	if (do_pkt_csum(rxcp, adapter->rx_csum))
@@ -981,23 +983,41 @@ static struct be_eth_tx_compl *be_tx_compl_get(struct be_queue_info *tx_cq)
 static void be_tx_compl_process(struct be_adapter *adapter, u16 last_index)
 {
 	struct be_queue_info *txq = &adapter->tx_obj.q;
+	struct be_eth_wrb *wrb;
 	struct sk_buff **sent_skbs = adapter->tx_obj.sent_skb_list;
 	struct sk_buff *sent_skb;
+	u64 busaddr;
 	u16 cur_index, num_wrbs = 0;
 
 	cur_index = txq->tail;
 	sent_skb = sent_skbs[cur_index];
 	BUG_ON(!sent_skb);
 	sent_skbs[cur_index] = NULL;
+	wrb = queue_tail_node(txq);
+	be_dws_le_to_cpu(wrb, sizeof(*wrb));
+	busaddr = ((u64)wrb->frag_pa_hi << 32) | (u64)wrb->frag_pa_lo;
+	if (busaddr != 0) {
+		pci_unmap_single(adapter->pdev, busaddr,
+				 wrb->frag_len, PCI_DMA_TODEVICE);
+	}
+	num_wrbs++;
+	queue_tail_inc(txq);
 
-	do {
+	while (cur_index != last_index) {
 		cur_index = txq->tail;
+		wrb = queue_tail_node(txq);
+		be_dws_le_to_cpu(wrb, sizeof(*wrb));
+		busaddr = ((u64)wrb->frag_pa_hi << 32) | (u64)wrb->frag_pa_lo;
+		if (busaddr != 0) {
+			pci_unmap_page(adapter->pdev, busaddr,
+				       wrb->frag_len, PCI_DMA_TODEVICE);
+		}
 		num_wrbs++;
 		queue_tail_inc(txq);
-	} while (cur_index != last_index);
+	}
 
 	atomic_sub(num_wrbs, &txq->used);
-	skb_dma_unmap(&adapter->pdev->dev, sent_skb, DMA_TO_DEVICE);
+
 	kfree_skb(sent_skb);
 }
 
@@ -1377,6 +1397,7 @@ int be_poll_rx(struct napi_struct *napi, int budget)
 	struct be_eth_rx_compl *rxcp;
 	u32 work_done;
 
+	adapter->stats.drvr_stats.be_rx_polls++;