Merge branch 'master' of git://git.kernel.org/pub/scm/linux/kernel/git/linville/wirel...
authorJohn W. Linville <linville@tuxdriver.com>
Mon, 29 Apr 2013 19:31:57 +0000 (15:31 -0400)
committerJohn W. Linville <linville@tuxdriver.com>
Mon, 29 Apr 2013 19:31:57 +0000 (15:31 -0400)
33 files changed:
drivers/bluetooth/btmrvl_debugfs.c
drivers/bluetooth/btmrvl_sdio.c
drivers/bluetooth/btusb.c
drivers/net/wireless/brcm80211/brcmsmac/mac80211_if.c
drivers/net/wireless/iwlwifi/dvm/agn.h
drivers/net/wireless/iwlwifi/dvm/debugfs.c
drivers/net/wireless/iwlwifi/dvm/main.c
drivers/net/wireless/iwlwifi/dvm/sta.c
drivers/net/wireless/iwlwifi/mvm/mac80211.c
drivers/net/wireless/iwlwifi/mvm/phy-ctxt.c
drivers/net/wireless/iwlwifi/mvm/power.c
drivers/net/wireless/iwlwifi/mvm/utils.c
drivers/net/wireless/iwlwifi/pcie/drv.c
drivers/net/wireless/mwifiex/pcie.c
drivers/net/wireless/rtlwifi/usb.c
drivers/ssb/driver_chipcommon_pmu.c
include/net/bluetooth/hci_core.h
net/bluetooth/hci_core.c
net/bluetooth/l2cap_core.c
net/bluetooth/mgmt.c
net/nfc/Kconfig
net/nfc/Makefile
net/nfc/llcp.h [new file with mode: 0644]
net/nfc/llcp/Kconfig [deleted file]
net/nfc/llcp/commands.c [deleted file]
net/nfc/llcp/llcp.c [deleted file]
net/nfc/llcp/llcp.h [deleted file]
net/nfc/llcp/sock.c [deleted file]
net/nfc/llcp_commands.c [new file with mode: 0644]
net/nfc/llcp_core.c [new file with mode: 0644]
net/nfc/llcp_sock.c [new file with mode: 0644]
net/nfc/netlink.c
net/nfc/nfc.h

index 428dbb7574bd75aa43d84ba5af353a134d1abfd3..db2c3c305df8ea183d499861089e7e446d2beabe 100644 (file)
 struct btmrvl_debugfs_data {
        struct dentry *config_dir;
        struct dentry *status_dir;
-
-       /* config */
-       struct dentry *psmode;
-       struct dentry *pscmd;
-       struct dentry *hsmode;
-       struct dentry *hscmd;
-       struct dentry *gpiogap;
-       struct dentry *hscfgcmd;
-
-       /* status */
-       struct dentry *curpsmode;
-       struct dentry *hsstate;
-       struct dentry *psstate;
-       struct dentry *txdnldready;
 };
 
 static ssize_t btmrvl_hscfgcmd_write(struct file *file,
@@ -91,47 +77,6 @@ static const struct file_operations btmrvl_hscfgcmd_fops = {
        .llseek = default_llseek,
 };
 
-static ssize_t btmrvl_psmode_write(struct file *file, const char __user *ubuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       long result, ret;
-
-       memset(buf, 0, sizeof(buf));
-
-       if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
-               return -EFAULT;
-
-       ret = strict_strtol(buf, 10, &result);
-       if (ret)
-               return ret;
-
-       priv->btmrvl_dev.psmode = result;
-
-       return count;
-}
-
-static ssize_t btmrvl_psmode_read(struct file *file, char __user *userbuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       int ret;
-
-       ret = snprintf(buf, sizeof(buf) - 1, "%d\n",
-                                               priv->btmrvl_dev.psmode);
-
-       return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_psmode_fops = {
-       .read   = btmrvl_psmode_read,
-       .write  = btmrvl_psmode_write,
-       .open   = simple_open,
-       .llseek = default_llseek,
-};
-
 static ssize_t btmrvl_pscmd_write(struct file *file, const char __user *ubuf,
                                                size_t count, loff_t *ppos)
 {
@@ -178,47 +123,6 @@ static const struct file_operations btmrvl_pscmd_fops = {
        .llseek = default_llseek,
 };
 
-static ssize_t btmrvl_gpiogap_write(struct file *file, const char __user *ubuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       long result, ret;
-
-       memset(buf, 0, sizeof(buf));
-
-       if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
-               return -EFAULT;
-
-       ret = strict_strtol(buf, 16, &result);
-       if (ret)
-               return ret;
-
-       priv->btmrvl_dev.gpio_gap = result;
-
-       return count;
-}
-
-static ssize_t btmrvl_gpiogap_read(struct file *file, char __user *userbuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       int ret;
-
-       ret = snprintf(buf, sizeof(buf) - 1, "0x%x\n",
-                                               priv->btmrvl_dev.gpio_gap);
-
-       return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_gpiogap_fops = {
-       .read   = btmrvl_gpiogap_read,
-       .write  = btmrvl_gpiogap_write,
-       .open   = simple_open,
-       .llseek = default_llseek,
-};
-
 static ssize_t btmrvl_hscmd_write(struct file *file, const char __user *ubuf,
                                                size_t count, loff_t *ppos)
 {
@@ -263,119 +167,6 @@ static const struct file_operations btmrvl_hscmd_fops = {
        .llseek = default_llseek,
 };
 
-static ssize_t btmrvl_hsmode_write(struct file *file, const char __user *ubuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       long result, ret;
-
-       memset(buf, 0, sizeof(buf));
-
-       if (copy_from_user(&buf, ubuf, min_t(size_t, sizeof(buf) - 1, count)))
-               return -EFAULT;
-
-       ret = strict_strtol(buf, 10, &result);
-       if (ret)
-               return ret;
-
-       priv->btmrvl_dev.hsmode = result;
-
-       return count;
-}
-
-static ssize_t btmrvl_hsmode_read(struct file *file, char __user * userbuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       int ret;
-
-       ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->btmrvl_dev.hsmode);
-
-       return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_hsmode_fops = {
-       .read   = btmrvl_hsmode_read,
-       .write  = btmrvl_hsmode_write,
-       .open   = simple_open,
-       .llseek = default_llseek,
-};
-
-static ssize_t btmrvl_curpsmode_read(struct file *file, char __user *userbuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       int ret;
-
-       ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->psmode);
-
-       return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_curpsmode_fops = {
-       .read   = btmrvl_curpsmode_read,
-       .open   = simple_open,
-       .llseek = default_llseek,
-};
-
-static ssize_t btmrvl_psstate_read(struct file *file, char __user * userbuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       int ret;
-
-       ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->ps_state);
-
-       return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_psstate_fops = {
-       .read   = btmrvl_psstate_read,
-       .open   = simple_open,
-       .llseek = default_llseek,
-};
-
-static ssize_t btmrvl_hsstate_read(struct file *file, char __user *userbuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       int ret;
-
-       ret = snprintf(buf, sizeof(buf) - 1, "%d\n", priv->adapter->hs_state);
-
-       return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_hsstate_fops = {
-       .read   = btmrvl_hsstate_read,
-       .open   = simple_open,
-       .llseek = default_llseek,
-};
-
-static ssize_t btmrvl_txdnldready_read(struct file *file, char __user *userbuf,
-                                               size_t count, loff_t *ppos)
-{
-       struct btmrvl_private *priv = file->private_data;
-       char buf[16];
-       int ret;
-
-       ret = snprintf(buf, sizeof(buf) - 1, "%d\n",
-                                       priv->btmrvl_dev.tx_dnld_rdy);
-
-       return simple_read_from_buffer(userbuf, count, ppos, buf, ret);
-}
-
-static const struct file_operations btmrvl_txdnldready_fops = {
-       .read   = btmrvl_txdnldready_read,
-       .open   = simple_open,
-       .llseek = default_llseek,
-};
-
 void btmrvl_debugfs_init(struct hci_dev *hdev)
 {
        struct btmrvl_private *priv = hci_get_drvdata(hdev);
@@ -394,30 +185,28 @@ void btmrvl_debugfs_init(struct hci_dev *hdev)
 
        dbg->config_dir = debugfs_create_dir("config", hdev->debugfs);
 
-       dbg->psmode = debugfs_create_file("psmode", 0644, dbg->config_dir,
-                                         priv, &btmrvl_psmode_fops);
-       dbg->pscmd = debugfs_create_file("pscmd", 0644, dbg->config_dir,
-                                        priv, &btmrvl_pscmd_fops);
-       dbg->gpiogap = debugfs_create_file("gpiogap", 0644, dbg->config_dir,
-                                          priv, &btmrvl_gpiogap_fops);
-       dbg->hsmode =  debugfs_create_file("hsmode", 0644, dbg->config_dir,
-                                          priv, &btmrvl_hsmode_fops);
-       dbg->hscmd = debugfs_create_file("hscmd", 0644, dbg->config_dir,
-                                        priv, &btmrvl_hscmd_fops);
-       dbg->hscfgcmd = debugfs_create_file("hscfgcmd", 0644, dbg->config_dir,
-                                           priv, &btmrvl_hscfgcmd_fops);
+       debugfs_create_u8("psmode", 0644, dbg->config_dir,
+                         &priv->btmrvl_dev.psmode);
+       debugfs_create_file("pscmd", 0644, dbg->config_dir,
+                           priv, &btmrvl_pscmd_fops);
+       debugfs_create_x16("gpiogap", 0644, dbg->config_dir,
+                          &priv->btmrvl_dev.gpio_gap);
+       debugfs_create_u8("hsmode", 0644, dbg->config_dir,
+                         &priv->btmrvl_dev.hsmode);
+       debugfs_create_file("hscmd", 0644, dbg->config_dir,
+                           priv, &btmrvl_hscmd_fops);
+       debugfs_create_file("hscfgcmd", 0644, dbg->config_dir,
+                           priv, &btmrvl_hscfgcmd_fops);
 
        dbg->status_dir = debugfs_create_dir("status", hdev->debugfs);
-       dbg->curpsmode = debugfs_create_file("curpsmode", 0444,
-                                            dbg->status_dir, priv,
-                                            &btmrvl_curpsmode_fops);
-       dbg->psstate = debugfs_create_file("psstate", 0444, dbg->status_dir,
-                                          priv, &btmrvl_psstate_fops);
-       dbg->hsstate = debugfs_create_file("hsstate", 0444, dbg->status_dir,
-                                          priv, &btmrvl_hsstate_fops);
-       dbg->txdnldready = debugfs_create_file("txdnldready", 0444,
-                                              dbg->status_dir, priv,
-                                              &btmrvl_txdnldready_fops);
+       debugfs_create_u8("curpsmode", 0444, dbg->status_dir,
+                         &priv->adapter->psmode);
+       debugfs_create_u8("psstate", 0444, dbg->status_dir,
+                         &priv->adapter->ps_state);
+       debugfs_create_u8("hsstate", 0444, dbg->status_dir,
+                         &priv->adapter->hs_state);
+       debugfs_create_u8("txdnldready", 0444, dbg->status_dir,
+                         &priv->btmrvl_dev.tx_dnld_rdy);
 }
 
 void btmrvl_debugfs_remove(struct hci_dev *hdev)
@@ -428,19 +217,8 @@ void btmrvl_debugfs_remove(struct hci_dev *hdev)
        if (!dbg)
                return;
 
-       debugfs_remove(dbg->psmode);
-       debugfs_remove(dbg->pscmd);
-       debugfs_remove(dbg->gpiogap);
-       debugfs_remove(dbg->hsmode);
-       debugfs_remove(dbg->hscmd);
-       debugfs_remove(dbg->hscfgcmd);
-       debugfs_remove(dbg->config_dir);
-
-       debugfs_remove(dbg->curpsmode);
-       debugfs_remove(dbg->psstate);
-       debugfs_remove(dbg->hsstate);
-       debugfs_remove(dbg->txdnldready);
-       debugfs_remove(dbg->status_dir);
+       debugfs_remove_recursive(dbg->config_dir);
+       debugfs_remove_recursive(dbg->status_dir);
 
        kfree(dbg);
 }
index 1cb51839912d0e791254349a64f171ec2a9754a2..c63488c54f4aa5e465f774a16f2f44f360723dc1 100644 (file)
@@ -228,24 +228,24 @@ failed:
 static int btmrvl_sdio_verify_fw_download(struct btmrvl_sdio_card *card,
                                                                int pollnum)
 {
-       int ret = -ETIMEDOUT;
        u16 firmwarestat;
-       unsigned int tries;
+       int tries, ret;
 
         /* Wait for firmware to become ready */
        for (tries = 0; tries < pollnum; tries++) {
-               if (btmrvl_sdio_read_fw_status(card, &firmwarestat) < 0)
+               sdio_claim_host(card->func);
+               ret = btmrvl_sdio_read_fw_status(card, &firmwarestat);
+               sdio_release_host(card->func);
+               if (ret < 0)
                        continue;
 
-               if (firmwarestat == FIRMWARE_READY) {
-                       ret = 0;
-                       break;
-               } else {
-                       msleep(10);
-               }
+               if (firmwarestat == FIRMWARE_READY)
+                       return 0;
+
+               msleep(10);
        }
 
-       return ret;
+       return -ETIMEDOUT;
 }
 
 static int btmrvl_sdio_download_helper(struct btmrvl_sdio_card *card)
@@ -874,7 +874,7 @@ exit:
 
 static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
 {
-       int ret = 0;
+       int ret;
        u8 fws0;
        int pollnum = MAX_POLL_TRIES;
 
@@ -882,13 +882,14 @@ static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
                BT_ERR("card or function is NULL!");
                return -EINVAL;
        }
-       sdio_claim_host(card->func);
 
        if (!btmrvl_sdio_verify_fw_download(card, 1)) {
                BT_DBG("Firmware already downloaded!");
-               goto done;
+               return 0;
        }
 
+       sdio_claim_host(card->func);
+
        /* Check if other function driver is downloading the firmware */
        fws0 = sdio_readb(card->func, card->reg->card_fw_status0, &ret);
        if (ret) {
@@ -918,15 +919,21 @@ static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
                }
        }
 
+       sdio_release_host(card->func);
+
+       /*
+        * winner or not, with this test the FW synchronizes when the
+        * module can continue its initialization
+        */
        if (btmrvl_sdio_verify_fw_download(card, pollnum)) {
                BT_ERR("FW failed to be active in time!");
-               ret = -ETIMEDOUT;
-               goto done;
+               return -ETIMEDOUT;
        }
 
+       return 0;
+
 done:
        sdio_release_host(card->func);
-
        return ret;
 }
 
@@ -989,8 +996,6 @@ static int btmrvl_sdio_probe(struct sdio_func *func,
                goto unreg_dev;
        }
 
-       msleep(100);
-
        btmrvl_sdio_enable_host_int(card);
 
        priv = btmrvl_add_card(card);
index 3d684d20b58476b1edf7ae6e744c6620a459a82e..7a7e5f8ecadcffef990eaea6c2fafdcf94ff4c4b 100644 (file)
@@ -23,6 +23,7 @@
 
 #include <linux/module.h>
 #include <linux/usb.h>
+#include <linux/firmware.h>
 
 #include <net/bluetooth/bluetooth.h>
 #include <net/bluetooth/hci_core.h>
@@ -47,6 +48,7 @@ static struct usb_driver btusb_driver;
 #define BTUSB_BROKEN_ISOC      0x20
 #define BTUSB_WRONG_SCO_MTU    0x40
 #define BTUSB_ATH3012          0x80
+#define BTUSB_INTEL            0x100
 
 static struct usb_device_id btusb_table[] = {
        /* Generic Bluetooth USB device */
@@ -207,6 +209,9 @@ static struct usb_device_id blacklist_table[] = {
        /* Frontline ComProbe Bluetooth Sniffer */
        { USB_DEVICE(0x16d3, 0x0002), .driver_info = BTUSB_SNIFFER },
 
+       /* Intel Bluetooth device */
+       { USB_DEVICE(0x8087, 0x07dc), .driver_info = BTUSB_INTEL },
+
        { }     /* Terminating entry */
 };
 
@@ -943,6 +948,375 @@ static int btusb_setup_bcm92035(struct hci_dev *hdev)
        return 0;
 }
 
+struct intel_version {
+       u8 status;
+       u8 hw_platform;
+       u8 hw_variant;
+       u8 hw_revision;
+       u8 fw_variant;
+       u8 fw_revision;
+       u8 fw_build_num;
+       u8 fw_build_ww;
+       u8 fw_build_yy;
+       u8 fw_patch_num;
+} __packed;
+
+static const struct firmware *btusb_setup_intel_get_fw(struct hci_dev *hdev,
+                                               struct intel_version *ver)
+{
+       const struct firmware *fw;
+       char fwname[64];
+       int ret;
+
+       snprintf(fwname, sizeof(fwname),
+                "intel/ibt-hw-%x.%x.%x-fw-%x.%x.%x.%x.%x.bseq",
+                ver->hw_platform, ver->hw_variant, ver->hw_revision,
+                ver->fw_variant,  ver->fw_revision, ver->fw_build_num,
+                ver->fw_build_ww, ver->fw_build_yy);
+
+       ret = request_firmware(&fw, fwname, &hdev->dev);
+       if (ret < 0) {
+               if (ret == -EINVAL) {
+                       BT_ERR("%s Intel firmware file request failed (%d)",
+                              hdev->name, ret);
+                       return NULL;
+               }
+
+               BT_ERR("%s failed to open Intel firmware file: %s(%d)",
+                      hdev->name, fwname, ret);
+
+               /* If the correct firmware patch file is not found, use the
+                * default firmware patch file instead
+                */
+               snprintf(fwname, sizeof(fwname), "intel/ibt-hw-%x.%x.bseq",
+                        ver->hw_platform, ver->hw_variant);
+               if (request_firmware(&fw, fwname, &hdev->dev) < 0) {
+                       BT_ERR("%s failed to open default Intel fw file: %s",
+                              hdev->name, fwname);
+                       return NULL;
+               }
+       }
+
+       BT_INFO("%s: Intel Bluetooth firmware file: %s", hdev->name, fwname);
+
+       return fw;
+}
+
+static int btusb_setup_intel_patching(struct hci_dev *hdev,
+                                     const struct firmware *fw,
+                                     const u8 **fw_ptr, int *disable_patch)
+{
+       struct sk_buff *skb;
+       struct hci_command_hdr *cmd;
+       const u8 *cmd_param;
+       struct hci_event_hdr *evt = NULL;
+       const u8 *evt_param = NULL;
+       int remain = fw->size - (*fw_ptr - fw->data);
+
+       /* The first byte indicates the types of the patch command or event.
+        * 0x01 means HCI command and 0x02 is HCI event. If the first bytes
+        * in the current firmware buffer doesn't start with 0x01 or
+        * the size of remain buffer is smaller than HCI command header,
+        * the firmware file is corrupted and it should stop the patching
+        * process.
+        */
+       if (remain > HCI_COMMAND_HDR_SIZE && *fw_ptr[0] != 0x01) {
+               BT_ERR("%s Intel fw corrupted: invalid cmd read", hdev->name);
+               return -EINVAL;
+       }
+       (*fw_ptr)++;
+       remain--;
+
+       cmd = (struct hci_command_hdr *)(*fw_ptr);
+       *fw_ptr += sizeof(*cmd);
+       remain -= sizeof(*cmd);
+
+       /* Ensure that the remain firmware data is long enough than the length
+        * of command parameter. If not, the firmware file is corrupted.
+        */
+       if (remain < cmd->plen) {
+               BT_ERR("%s Intel fw corrupted: invalid cmd len", hdev->name);
+               return -EFAULT;
+       }
+
+       /* If there is a command that loads a patch in the firmware
+        * file, then enable the patch upon success, otherwise just
+        * disable the manufacturer mode, for example patch activation
+        * is not required when the default firmware patch file is used
+        * because there are no patch data to load.
+        */
+       if (*disable_patch && le16_to_cpu(cmd->opcode) == 0xfc8e)
+               *disable_patch = 0;
+
+       cmd_param = *fw_ptr;
+       *fw_ptr += cmd->plen;
+       remain -= cmd->plen;
+
+       /* This reads the expected events when the above command is sent to the
+        * device. Some vendor commands expects more than one events, for
+        * example command status event followed by vendor specific event.
+        * For this case, it only keeps the last expected event. so the command
+        * can be sent with __hci_cmd_sync_ev() which returns the sk_buff of
+        * last expected event.
+        */
+       while (remain > HCI_EVENT_HDR_SIZE && *fw_ptr[0] == 0x02) {
+               (*fw_ptr)++;
+               remain--;
+
+               evt = (struct hci_event_hdr *)(*fw_ptr);
+               *fw_ptr += sizeof(*evt);
+               remain -= sizeof(*evt);
+
+               if (remain < evt->plen) {
+                       BT_ERR("%s Intel fw corrupted: invalid evt len",
+                              hdev->name);
+                       return -EFAULT;
+               }
+
+               evt_param = *fw_ptr;
+               *fw_ptr += evt->plen;
+               remain -= evt->plen;
+       }
+
+       /* Every HCI commands in the firmware file has its correspond event.
+        * If event is not found or remain is smaller than zero, the firmware
+        * file is corrupted.
+        */
+       if (!evt || !evt_param || remain < 0) {
+               BT_ERR("%s Intel fw corrupted: invalid evt read", hdev->name);
+               return -EFAULT;
+       }
+
+       skb = __hci_cmd_sync_ev(hdev, le16_to_cpu(cmd->opcode), cmd->plen,
+                               cmd_param, evt->evt, HCI_INIT_TIMEOUT);
+       if (IS_ERR(skb)) {
+               BT_ERR("%s sending Intel patch command (0x%4.4x) failed (%ld)",
+                      hdev->name, cmd->opcode, PTR_ERR(skb));
+               return -PTR_ERR(skb);
+       }
+
+       /* It ensures that the returned event matches the event data read from
+        * the firmware file. At fist, it checks the length and then
+        * the contents of the event.
+        */
+       if (skb->len != evt->plen) {
+               BT_ERR("%s mismatch event length (opcode 0x%4.4x)", hdev->name,
+                      le16_to_cpu(cmd->opcode));
+               kfree_skb(skb);
+               return -EFAULT;
+       }
+
+       if (memcmp(skb->data, evt_param, evt->plen)) {
+               BT_ERR("%s mismatch event parameter (opcode 0x%4.4x)",
+                      hdev->name, le16_to_cpu(cmd->opcode));
+               kfree_skb(skb);
+               return -EFAULT;
+       }
+       kfree_skb(skb);
+
+       return 0;
+}
+
+static int btusb_setup_intel(struct hci_dev *hdev)
+{
+       struct sk_buff *skb;
+       const struct firmware *fw;
+       const u8 *fw_ptr;
+       int disable_patch;
+       struct intel_version *ver;
+
+       const u8 mfg_enable[] = { 0x01, 0x00 };
+       const u8 mfg_disable[] = { 0x00, 0x00 };
+       const u8 mfg_reset_deactivate[] = { 0x00, 0x01 };
+       const u8 mfg_reset_activate[] = { 0x00, 0x02 };
+
+       BT_DBG("%s", hdev->name);
+
+       /* The controller has a bug with the first HCI command sent to it
+        * returning number of completed commands as zero. This would stall the
+        * command processing in the Bluetooth core.
+        *
+        * As a workaround, send HCI Reset command first which will reset the
+        * number of completed commands and allow normal command processing
+        * from now on.
+        */
+       skb = __hci_cmd_sync(hdev, HCI_OP_RESET, 0, NULL, HCI_INIT_TIMEOUT);
+       if (IS_ERR(skb)) {
+               BT_ERR("%s sending initial HCI reset command failed (%ld)",
+                      hdev->name, PTR_ERR(skb));
+               return -PTR_ERR(skb);
+       }
+       kfree_skb(skb);
+
+       /* Read Intel specific controller version first to allow selection of
+        * which firmware file to load.
+        *
+        * The returned information are hardware variant and revision plus
+        * firmware variant, revision and build number.
+        */
+       skb = __hci_cmd_sync(hdev, 0xfc05, 0, NULL, HCI_INIT_TIMEOUT);
+       if (IS_ERR(skb)) {
+               BT_ERR("%s reading Intel fw version command failed (%ld)",
+                      hdev->name, PTR_ERR(skb));
+               return -PTR_ERR(skb);
+       }
+
+       if (skb->len != sizeof(*ver)) {
+               BT_ERR("%s Intel version event length mismatch", hdev->name);
+               kfree_skb(skb);
+               return -EIO;
+       }
+
+       ver = (struct intel_version *)skb->data;
+       if (ver->status) {
+               BT_ERR("%s Intel fw version event failed (%02x)", hdev->name,
+                      ver->status);
+               kfree_skb(skb);
+               return -bt_to_errno(ver->status);
+       }
+
+       BT_INFO("%s: read Intel version: %02x%02x%02x%02x%02x%02x%02x%02x%02x",
+               hdev->name, ver->hw_platform, ver->hw_variant,
+               ver->hw_revision, ver->fw_variant,  ver->fw_revision,
+               ver->fw_build_num, ver->fw_build_ww, ver->fw_build_yy,
+               ver->fw_patch_num);
+
+       /* fw_patch_num indicates the version of patch the device currently
+        * have. If there is no patch data in the device, it is always 0x00.
+        * So, if it is other than 0x00, no need to patch the deivce again.
+        */
+       if (ver->fw_patch_num) {
+               BT_INFO("%s: Intel device is already patched. patch num: %02x",
+                       hdev->name, ver->fw_patch_num);
+               kfree_skb(skb);
+               return 0;
+       }
+
+       /* Opens the firmware patch file based on the firmware version read
+        * from the controller. If it fails to open the matching firmware
+        * patch file, it tries to open the default firmware patch file.
+        * If no patch file is found, allow the device to operate without
+        * a patch.
+        */
+       fw = btusb_setup_intel_get_fw(hdev, ver);
+       if (!fw) {
+               kfree_skb(skb);
+               return 0;
+       }
+       fw_ptr = fw->data;
+
+       /* This Intel specific command enables the manufacturer mode of the
+        * controller.
+        *
+        * Only while this mode is enabled, the driver can download the
+        * firmware patch data and configuration parameters.
+        */
+       skb = __hci_cmd_sync(hdev, 0xfc11, 2, mfg_enable, HCI_INIT_TIMEOUT);
+       if (IS_ERR(skb)) {
+               BT_ERR("%s entering Intel manufacturer mode failed (%ld)",
+                      hdev->name, PTR_ERR(skb));
+               release_firmware(fw);
+               return -PTR_ERR(skb);
+       }
+
+       if (skb->data[0]) {
+               u8 evt_status = skb->data[0];
+               BT_ERR("%s enable Intel manufacturer mode event failed (%02x)",
+                      hdev->name, evt_status);
+               kfree_skb(skb);
+               release_firmware(fw);
+               return -bt_to_errno(evt_status);
+       }
+       kfree_skb(skb);
+
+       disable_patch = 1;
+
+       /* The firmware data file consists of list of Intel specific HCI
+        * commands and its expected events. The first byte indicates the
+        * type of the message, either HCI command or HCI event.
+        *
+        * It reads the command and its expected event from the firmware file,
+        * and send to the controller. Once __hci_cmd_sync_ev() returns,
+        * the returned event is compared with the event read from the firmware
+        * file and it will continue until all the messages are downloaded to
+        * the controller.
+        *
+        * Once the firmware patching is completed successfully,
+        * the manufacturer mode is disabled with reset and activating the
+        * downloaded patch.
+        *
+        * If the firmware patching fails, the manufacturer mode is
+        * disabled with reset and deactivating the patch.
+        *
+        * If the default patch file is used, no reset is done when disabling
+        * the manufacturer.
+        */
+       while (fw->size > fw_ptr - fw->data) {
+               int ret;
+
+               ret = btusb_setup_intel_patching(hdev, fw, &fw_ptr,
+                                                &disable_patch);
+               if (ret < 0)
+                       goto exit_mfg_deactivate;
+       }
+
+       release_firmware(fw);
+
+       if (disable_patch)
+               goto exit_mfg_disable;
+
+       /* Patching completed successfully and disable the manufacturer mode
+        * with reset and activate the downloaded firmware patches.
+        */
+       skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_reset_activate),
+                            mfg_reset_activate, HCI_INIT_TIMEOUT);
+       if (IS_ERR(skb)) {
+               BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
+                      hdev->name, PTR_ERR(skb));
+               return -PTR_ERR(skb);
+       }
+       kfree_skb(skb);
+
+       BT_INFO("%s: Intel Bluetooth firmware patch completed and activated",
+               hdev->name);
+
+       return 0;
+
+exit_mfg_disable:
+       /* Disable the manufacturer mode without reset */
+       skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_disable), mfg_disable,
+                            HCI_INIT_TIMEOUT);
+       if (IS_ERR(skb)) {
+               BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
+                      hdev->name, PTR_ERR(skb));
+               return -PTR_ERR(skb);
+       }
+       kfree_skb(skb);
+
+       BT_INFO("%s: Intel Bluetooth firmware patch completed", hdev->name);
+       return 0;
+
+exit_mfg_deactivate:
+       release_firmware(fw);
+
+       /* Patching failed. Disable the manufacturer mode with reset and
+        * deactivate the downloaded firmware patches.
+        */
+       skb = __hci_cmd_sync(hdev, 0xfc11, sizeof(mfg_reset_deactivate),
+                            mfg_reset_deactivate, HCI_INIT_TIMEOUT);
+       if (IS_ERR(skb)) {
+               BT_ERR("%s exiting Intel manufacturer mode failed (%ld)",
+                      hdev->name, PTR_ERR(skb));
+               return -PTR_ERR(skb);
+       }
+       kfree_skb(skb);
+
+       BT_INFO("%s: Intel Bluetooth firmware patch completed and deactivated",
+               hdev->name);
+
+       return 0;
+}
+
 static int btusb_probe(struct usb_interface *intf,
                                const struct usb_device_id *id)
 {
@@ -1048,6 +1422,9 @@ static int btusb_probe(struct usb_interface *intf,
        if (id->driver_info & BTUSB_BCM92035)
                hdev->setup = btusb_setup_bcm92035;
 
+       if (id->driver_info & BTUSB_INTEL)
+               hdev->setup = btusb_setup_intel;
+
        /* Interface numbers are hardcoded in the specification */
        data->isoc = usb_ifnum_to_if(data->udev, 1);
 
index 2346821667e6a715b7d2c8d62b8fb97977dfd2b9..3a6544710c8ab222cc126ef53d5f19143498cc9e 100644 (file)
@@ -336,6 +336,7 @@ static void brcms_remove(struct bcma_device *pdev)
        struct brcms_info *wl = hw->priv;
 
        if (wl->wlc) {
+               brcms_led_unregister(wl);
                wiphy_rfkill_set_hw_state(wl->pub->ieee_hw->wiphy, false);
                wiphy_rfkill_stop_polling(wl->pub->ieee_hw->wiphy);
                ieee80211_unregister_hw(hw);
index e575b9b0cda8542c24693ce591272cd99ca1f486..48545ab003110559f4c5d097024c1066afbcd530 100644 (file)
@@ -172,7 +172,7 @@ int iwl_calib_set(struct iwl_priv *priv,
                  const struct iwl_calib_hdr *cmd, int len);
 void iwl_calib_free_results(struct iwl_priv *priv);
 int iwl_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
-                           char **buf, bool display);
+                           char **buf);
 int iwlagn_hw_valid_rtc_data_addr(u32 addr);
 
 /* lib */
index 7b8178be119f4d0a110a2beaae3a91165df45bf1..d5329489245a74ccb5cac363e3831e36648c1ce3 100644 (file)
@@ -2237,15 +2237,13 @@ static ssize_t iwl_dbgfs_log_event_read(struct file *file,
                                         size_t count, loff_t *ppos)
 {
        struct iwl_priv *priv = file->private_data;
-       char *buf;
-       int pos = 0;
-       ssize_t ret = -ENOMEM;
+       char *buf = NULL;
+       ssize_t ret;
 
-       ret = pos = iwl_dump_nic_event_log(priv, true, &buf, true);
-       if (buf) {
-               ret = simple_read_from_buffer(user_buf, count, ppos, buf, pos);
-               kfree(buf);
-       }
+       ret = iwl_dump_nic_event_log(priv, true, &buf);
+       if (ret > 0)
+               ret = simple_read_from_buffer(user_buf, count, ppos, buf, ret);
+       kfree(buf);
        return ret;
 }
 
@@ -2269,7 +2267,7 @@ static ssize_t iwl_dbgfs_log_event_write(struct file *file,
        if (sscanf(buf, "%d", &event_log_flag) != 1)
                return -EFAULT;
        if (event_log_flag == 1)
-               iwl_dump_nic_event_log(priv, true, NULL, false);
+               iwl_dump_nic_event_log(priv, true, NULL);
 
        return count;
 }
index b9e3517652d649f4514ea9b3608f479cc5c81991..74d7572e709125160f0a0db02b8e337d23d96cc5 100644 (file)
@@ -1795,7 +1795,7 @@ static int iwl_print_last_event_logs(struct iwl_priv *priv, u32 capacity,
 #define DEFAULT_DUMP_EVENT_LOG_ENTRIES (20)
 
 int iwl_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
-                           char **buf, bool display)
+                           char **buf)
 {
        u32 base;       /* SRAM byte address of event log header */
        u32 capacity;   /* event log capacity in # entries */
@@ -1866,7 +1866,7 @@ int iwl_dump_nic_event_log(struct iwl_priv *priv, bool full_log,
                size);
 
 #ifdef CONFIG_IWLWIFI_DEBUG
-       if (display) {
+       if (buf) {
                if (full_log)
                        bufsz = capacity * 48;
                else
@@ -1962,7 +1962,7 @@ static void iwl_nic_error(struct iwl_op_mode *op_mode)
                priv->fw->fw_version);
 
        iwl_dump_nic_error_log(priv);
-       iwl_dump_nic_event_log(priv, false, NULL, false);
+       iwl_dump_nic_event_log(priv, false, NULL);
 
        iwlagn_fw_error(priv, false);
 }
index b775769f8322c437558712af955332296e95db89..db183b44e038ba7f1ed5a63f31bf7497e3599428 100644 (file)
@@ -695,6 +695,7 @@ void iwl_clear_ucode_stations(struct iwl_priv *priv,
 void iwl_restore_stations(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
 {
        struct iwl_addsta_cmd sta_cmd;
+       static const struct iwl_link_quality_cmd zero_lq = {};
        struct iwl_link_quality_cmd lq;
        int i;
        bool found = false;
@@ -733,7 +734,9 @@ void iwl_restore_stations(struct iwl_priv *priv, struct iwl_rxon_context *ctx)
                                else
                                        memcpy(&lq, priv->stations[i].lq,
                                               sizeof(struct iwl_link_quality_cmd));
-                               send_lq = true;
+
+                               if (!memcmp(&lq, &zero_lq, sizeof(lq)))
+                                       send_lq = true;
                        }
                        spin_unlock_bh(&priv->sta_lock);
                        ret = iwl_send_add_sta(priv, &sta_cmd, CMD_SYNC);
index fe031608fd912ce67201358a187031a09c8cb549..dd158ec571fbb183374858e20ffd80325206db9d 100644 (file)
@@ -207,7 +207,7 @@ int iwl_mvm_mac_setup_register(struct iwl_mvm *mvm)
 
        hw->wiphy->hw_version = mvm->trans->hw_id;
 
-       if (iwlwifi_mod_params.power_save)
+       if (iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM)
                hw->wiphy->flags |= WIPHY_FLAG_PS_ON_BY_DEFAULT;
        else
                hw->wiphy->flags &= ~WIPHY_FLAG_PS_ON_BY_DEFAULT;
index 0f0b44eabd937a88ce615e3f6a5fe5ada0fe805f..a28a1d1f23eb8caba29ff4501c1962a527f6e50d 100644 (file)
@@ -153,11 +153,6 @@ static void iwl_mvm_phy_ctxt_cmd_data(struct iwl_mvm *mvm,
        cmd->ci.ctrl_pos = iwl_mvm_get_ctrl_pos(chandef);
 
        /* Set rx the chains */
-
-       /* TODO:
-        * Need to add on chain noise calibration limitations, and
-        * BT coex considerations.
-        */
        idle_cnt = chains_static;
        active_cnt = chains_dynamic;
 
index 9395ab2a1af2cc60838542eb312428e9c31aa0c7..ed77e437aac49d513525b12725f3d70baad461e9 100644 (file)
@@ -111,8 +111,7 @@ void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
         */
        cmd->keep_alive_seconds = POWER_KEEP_ALIVE_PERIOD_SEC;
 
-       if ((iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM) ||
-           !iwlwifi_mod_params.power_save)
+       if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_CAM)
                return;
 
        cmd->flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
@@ -146,14 +145,8 @@ void iwl_mvm_power_build_cmd(struct iwl_mvm *mvm, struct ieee80211_vif *vif,
        keep_alive = DIV_ROUND_UP(keep_alive, MSEC_PER_SEC);
        cmd->keep_alive_seconds = keep_alive;
 
-       if (iwlmvm_mod_params.power_scheme == IWL_POWER_SCHEME_LP) {
-               /* TODO: Also for D3 (device sleep / WoWLAN) */
-               cmd->rx_data_timeout = cpu_to_le32(10 * USEC_PER_MSEC);
-               cmd->tx_data_timeout = cpu_to_le32(10 * USEC_PER_MSEC);
-       } else {
-               cmd->rx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
-               cmd->tx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
-       }
+       cmd->rx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
+       cmd->tx_data_timeout = cpu_to_le32(100 * USEC_PER_MSEC);
 }
 
 int iwl_mvm_power_update_mode(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
@@ -177,8 +170,7 @@ int iwl_mvm_power_disable(struct iwl_mvm *mvm, struct ieee80211_vif *vif)
        if (vif->type != NL80211_IFTYPE_STATION || vif->p2p)
                return 0;
 
-       if ((iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM) &&
-           iwlwifi_mod_params.power_save)
+       if (iwlmvm_mod_params.power_scheme != IWL_POWER_SCHEME_CAM)
                cmd.flags |= cpu_to_le16(POWER_FLAGS_POWER_SAVE_ENA_MSK);
 
        iwl_mvm_power_log(mvm, &cmd);
index 0cc8d8c0d39372f22752130dea146fc8b8e5ffd8..687b34e387ac325b0b5b5a3cf869625d31341ad4 100644 (file)
@@ -253,8 +253,9 @@ int iwl_mvm_rx_fw_error(struct iwl_mvm *mvm, struct iwl_rx_cmd_buffer *rxb,
 u8 first_antenna(u8 mask)
 {
        BUILD_BUG_ON(ANT_A != BIT(0)); /* using ffs is wrong if not */
-       WARN_ON_ONCE(!mask); /* ffs will return 0 if mask is zeroed */
-       return (u8)(BIT(ffs(mask)));
+       if (WARN_ON_ONCE(!mask)) /* ffs will return 0 if mask is zeroed */
+               return BIT(0);
+       return BIT(ffs(mask) - 1);
 }
 
 /*
index 0016bb24b3d7c874cd7da3fcb32a23d64190d6a2..8cb53ec2b77bc6534faf95efaf64377e80ceb798 100644 (file)
@@ -256,6 +256,7 @@ static DEFINE_PCI_DEVICE_TABLE(iwl_hw_card_ids) = {
 
 /* 7000 Series */
        {IWL_PCI_DEVICE(0x08B1, 0x4070, iwl7260_2ac_cfg)},
+       {IWL_PCI_DEVICE(0x08B1, 0x4062, iwl7260_2ac_cfg)},
        {IWL_PCI_DEVICE(0x08B1, 0xC070, iwl7260_2ac_cfg)},
        {IWL_PCI_DEVICE(0x08B3, 0x0070, iwl3160_ac_cfg)},
        {IWL_PCI_DEVICE(0x08B3, 0x8070, iwl3160_ac_cfg)},
index 80f282c0bd4d0a4dcf9f159f6c831b91a0003f8d..20c9c4c7b0b2eb64fa75bd4180fd85015d77b33e 100644 (file)
@@ -861,9 +861,8 @@ static int mwifiex_pcie_delete_cmdrsp_buf(struct mwifiex_adapter *adapter)
 
        if (card && card->cmd_buf) {
                MWIFIEX_SKB_PACB(card->cmd_buf, &buf_pa);
-               pci_unmap_single(card->dev, buf_pa, MWIFIEX_SIZE_OF_CMD_BUFFER,
+               pci_unmap_single(card->dev, buf_pa, card->cmd_buf->len,
                                 PCI_DMA_TODEVICE);
-               dev_kfree_skb_any(card->cmd_buf);
        }
        return 0;
 }
@@ -1573,7 +1572,7 @@ static int mwifiex_pcie_cmdrsp_complete(struct mwifiex_adapter *adapter,
        skb_tmp = card->cmd_buf;
        if (skb_tmp) {
                MWIFIEX_SKB_PACB(skb_tmp, &buf_pa);
-               pci_unmap_single(card->dev, buf_pa, MWIFIEX_UPLD_SIZE,
+               pci_unmap_single(card->dev, buf_pa, skb_tmp->len,
                                 PCI_DMA_FROMDEVICE);
                card->cmd_buf = NULL;
        }
@@ -2294,9 +2293,9 @@ static void mwifiex_pcie_cleanup(struct mwifiex_adapter *adapter)
        if (pdev) {
                pci_iounmap(pdev, card->pci_mmap);
                pci_iounmap(pdev, card->pci_mmap1);
-
-               pci_release_regions(pdev);
                pci_disable_device(pdev);
+               pci_release_region(pdev, 2);
+               pci_release_region(pdev, 0);
                pci_set_drvdata(pdev, NULL);
        }
 }
index 83915dcd0e58505ca8970352e2fbac35dd1d48a8..76732b0cd221f13d937297f8fd1e4c08fd0c2d89 100644 (file)
@@ -521,6 +521,9 @@ static void _rtl_usb_rx_process_noagg(struct ieee80211_hw *hw,
                                rtlpriv->link_info.num_rx_inperiod++;
                }
 
+               /* static bcn for roaming */
+               rtl_beacon_statistic(hw, skb);
+
                if (likely(rtl_action_proc(hw, skb, false)))
                        ieee80211_rx(hw, skb);
                else
index 23c5dbfea115acdf9a2272c365fcc0ecbf89d14c..1173a091b402e1b73984d945cce2c2358cbc638f 100644 (file)
@@ -687,8 +687,23 @@ void ssb_pmu_spuravoid_pllupdate(struct ssb_chipcommon *cc, int spuravoid)
                pmu_ctl = SSB_CHIPCO_PMU_CTL_PLL_UPD;
                break;
        case 43222:
-               /* TODO: BCM43222 requires updating PLLs too */
-               return;
+               if (spuravoid == 1) {
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL0, 0x11500008);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL1, 0x0C000C06);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL2, 0x0F600a08);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL3, 0x00000000);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL4, 0x2001E920);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL5, 0x88888815);
+               } else {
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL0, 0x11100008);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL1, 0x0c000c06);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL2, 0x03000a08);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL3, 0x00000000);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL4, 0x200005c0);
+                       ssb_chipco_pll_write(cc, SSB_PMU1_PLLCTL5, 0x88888855);
+               }
+               pmu_ctl = SSB_CHIPCO_PMU_CTL_PLL_UPD;
+               break;
        default:
                ssb_printk(KERN_ERR PFX
                           "Unknown spuravoidance settings for chip 0x%04X, not changing PLL\n",
index 80d718a9b31f349df529674c15ac29c04e93bb98..35a57cd1704c30576a76fb9758855f6ba333abcc 100644 (file)
@@ -1081,17 +1081,19 @@ struct hci_request {
 
 void hci_req_init(struct hci_request *req, struct hci_dev *hdev);
 int hci_req_run(struct hci_request *req, hci_req_complete_t complete);
-void hci_req_add(struct hci_request *req, u16 opcode, u32 plen, void *param);
-void hci_req_add_ev(struct hci_request *req, u16 opcode, u32 plen, void *param,
-                   u8 event);
+void hci_req_add(struct hci_request *req, u16 opcode, u32 plen,
+                const void *param);
+void hci_req_add_ev(struct hci_request *req, u16 opcode, u32 plen,
+                   const void *param, u8 event);
 void hci_req_cmd_complete(struct hci_dev *hdev, u16 opcode, u8 status);
 
 struct sk_buff *__hci_cmd_sync(struct hci_dev *hdev, u16 opcode, u32 plen,
-                              void *param, u32 timeout);
+                              const void *param, u32 timeout);
 struct sk_buff *__hci_cmd_sync_ev(struct hci_dev *hdev, u16 opcode, u32 plen,
-                                 void *param, u8 event, u32 timeout);
+                                 const void *param, u8 event, u32 timeout);
 
-int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param);
+int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
+                const void *param);
 void hci_send_acl(struct hci_chan *chan, struct sk_buff *skb, __u16 flags);
 void hci_send_sco(struct hci_conn *conn, struct sk_buff *skb);
 
index ce82265f56190af81092fe0fe8a0e0426b486051..33843c5c49398f0e2a373de8919cde67a840451a 100644 (file)
@@ -79,7 +79,8 @@ static void hci_req_cancel(struct hci_dev *hdev, int err)
        }
 }
 
-struct sk_buff *hci_get_cmd_complete(struct hci_dev *hdev, u16 opcode, u8 event)
+static struct sk_buff *hci_get_cmd_complete(struct hci_dev *hdev, u16 opcode,
+                                           u8 event)
 {
        struct hci_ev_cmd_complete *ev;
        struct hci_event_hdr *hdr;
@@ -134,7 +135,7 @@ failed:
 }
 
 struct sk_buff *__hci_cmd_sync_ev(struct hci_dev *hdev, u16 opcode, u32 plen,
-                                 void *param, u8 event, u32 timeout)
+                                 const void *param, u8 event, u32 timeout)
 {
        DECLARE_WAITQUEUE(wait, current);
        struct hci_request req;
@@ -188,7 +189,7 @@ struct sk_buff *__hci_cmd_sync_ev(struct hci_dev *hdev, u16 opcode, u32 plen,
 EXPORT_SYMBOL(__hci_cmd_sync_ev);
 
 struct sk_buff *__hci_cmd_sync(struct hci_dev *hdev, u16 opcode, u32 plen,
-                              void *param, u32 timeout)
+                              const void *param, u32 timeout)
 {
        return __hci_cmd_sync_ev(hdev, opcode, plen, param, 0, timeout);
 }
@@ -377,6 +378,8 @@ static void bredr_setup(struct hci_request *req)
 
 static void le_setup(struct hci_request *req)
 {
+       struct hci_dev *hdev = req->hdev;
+
        /* Read LE Buffer Size */
        hci_req_add(req, HCI_OP_LE_READ_BUFFER_SIZE, 0, NULL);
 
@@ -391,6 +394,10 @@ static void le_setup(struct hci_request *req)
 
        /* Read LE Supported States */
        hci_req_add(req, HCI_OP_LE_READ_SUPPORTED_STATES, 0, NULL);
+
+       /* LE-only controllers have LE implicitly enabled */
+       if (!lmp_bredr_capable(hdev))
+               set_bit(HCI_LE_ENABLED, &hdev->dev_flags);
 }
 
 static u8 hci_get_inquiry_mode(struct hci_dev *hdev)
@@ -574,6 +581,10 @@ static void hci_set_le_support(struct hci_request *req)
        struct hci_dev *hdev = req->hdev;
        struct hci_cp_write_le_host_supported cp;
 
+       /* LE-only devices do not support explicit enablement */
+       if (!lmp_bredr_capable(hdev))
+               return;
+
        memset(&cp, 0, sizeof(cp));
 
        if (test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) {
@@ -2602,7 +2613,7 @@ int hci_req_run(struct hci_request *req, hci_req_complete_t complete)
 }
 
 static struct sk_buff *hci_prepare_cmd(struct hci_dev *hdev, u16 opcode,
-                                      u32 plen, void *param)
+                                      u32 plen, const void *param)
 {
        int len = HCI_COMMAND_HDR_SIZE + plen;
        struct hci_command_hdr *hdr;
@@ -2628,7 +2639,8 @@ static struct sk_buff *hci_prepare_cmd(struct hci_dev *hdev, u16 opcode,
 }
 
 /* Send HCI command */
-int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
+int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen,
+                const void *param)
 {
        struct sk_buff *skb;
 
@@ -2652,8 +2664,8 @@ int hci_send_cmd(struct hci_dev *hdev, __u16 opcode, __u32 plen, void *param)
 }
 
 /* Queue a command to an asynchronous HCI request */
-void hci_req_add_ev(struct hci_request *req, u16 opcode, u32 plen, void *param,
-                   u8 event)
+void hci_req_add_ev(struct hci_request *req, u16 opcode, u32 plen,
+                   const void *param, u8 event)
 {
        struct hci_dev *hdev = req->hdev;
        struct sk_buff *skb;
@@ -2682,7 +2694,8 @@ void hci_req_add_ev(struct hci_request *req, u16 opcode, u32 plen, void *param,
        skb_queue_tail(&req->cmd_q, skb);
 }
 
-void hci_req_add(struct hci_request *req, u16 opcode, u32 plen, void *param)
+void hci_req_add(struct hci_request *req, u16 opcode, u32 plen,
+                const void *param)
 {
        hci_req_add_ev(req, opcode, plen, param, 0);
 }
index eae1d9f90b68adab6dc9cf3de035c86219be1309..a76d1ac0321b40f48f6de231f31ff14e2af363df 100644 (file)
@@ -6314,12 +6314,13 @@ drop:
        kfree_skb(skb);
 }
 
-static void l2cap_att_channel(struct l2cap_conn *conn, u16 cid,
+static void l2cap_att_channel(struct l2cap_conn *conn,
                              struct sk_buff *skb)
 {
        struct l2cap_chan *chan;
 
-       chan = l2cap_global_chan_by_scid(0, cid, conn->src, conn->dst);
+       chan = l2cap_global_chan_by_scid(0, L2CAP_CID_LE_DATA,
+                                        conn->src, conn->dst);
        if (!chan)
                goto drop;
 
@@ -6368,7 +6369,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
                break;
 
        case L2CAP_CID_LE_DATA:
-               l2cap_att_channel(conn, cid, skb);
+               l2cap_att_channel(conn, skb);
                break;
 
        case L2CAP_CID_SMP:
index 4c830c62ef7412a8b4888b7a775b68117591240a..35fef22703e9dc3661c88d3adb5b5abc9532c463 100644 (file)
@@ -1351,6 +1351,11 @@ static int set_le(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
                return cmd_status(sk, hdev->id, MGMT_OP_SET_LE,
                                  MGMT_STATUS_INVALID_PARAMS);
 
+       /* LE-only devices do not allow toggling LE on/off */
+       if (!lmp_bredr_capable(hdev))
+               return cmd_status(sk, hdev->id, MGMT_OP_SET_LE,
+                                 MGMT_STATUS_REJECTED);
+
        hci_dev_lock(hdev);
 
        val = !!cp->val;
@@ -3347,7 +3352,8 @@ static int powered_update_hci(struct hci_dev *hdev)
                hci_req_add(&req, HCI_OP_WRITE_SSP_MODE, 1, &ssp);
        }
 
-       if (test_bit(HCI_LE_ENABLED, &hdev->dev_flags)) {
+       if (test_bit(HCI_LE_ENABLED, &hdev->dev_flags) &&
+           lmp_bredr_capable(hdev)) {
                struct hci_cp_write_le_host_supported cp;
 
                cp.le = 1;
index 60c3bbb63e8efaf1ea9de16e32641de804235829..5948b2fc72f63d3a9994e962b1366f8913c61ed7 100644 (file)
@@ -4,6 +4,7 @@
 
 menuconfig NFC
        depends on NET
+       depends on RFKILL || !RFKILL
        tristate "NFC subsystem support"
        default n
        help
@@ -15,6 +16,5 @@ menuconfig NFC
 
 source "net/nfc/nci/Kconfig"
 source "net/nfc/hci/Kconfig"
-source "net/nfc/llcp/Kconfig"
 
 source "drivers/nfc/Kconfig"
index d1a117c2c401720864b0566bd4fca0ef82bf6fbf..fb799deaed4f9d5b130e88ed5402190b33fea1f6 100644 (file)
@@ -5,6 +5,8 @@
 obj-$(CONFIG_NFC) += nfc.o
 obj-$(CONFIG_NFC_NCI) += nci/
 obj-$(CONFIG_NFC_HCI) += hci/
+#obj-$(CONFIG_NFC_LLCP) += llcp/
+
+nfc-objs := core.o netlink.o af_nfc.o rawsock.o llcp_core.o llcp_commands.o \
+               llcp_sock.o
 
-nfc-objs := core.o netlink.o af_nfc.o rawsock.o
-nfc-$(CONFIG_NFC_LLCP) += llcp/llcp.o llcp/commands.o llcp/sock.o
diff --git a/net/nfc/llcp.h b/net/nfc/llcp.h
new file mode 100644 (file)
index 0000000..ff8c434
--- /dev/null
@@ -0,0 +1,267 @@
+/*
+ * Copyright (C) 2011  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 as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+enum llcp_state {
+       LLCP_CONNECTED = 1, /* wait_for_packet() wants that */
+       LLCP_CLOSED,
+       LLCP_BOUND,
+       LLCP_LISTEN,
+};
+
+#define LLCP_DEFAULT_LTO 100
+#define LLCP_DEFAULT_RW  1
+#define LLCP_DEFAULT_MIU 128
+
+#define LLCP_MAX_LTO  0xff
+#define LLCP_MAX_RW   15
+#define LLCP_MAX_MIUX 0x7ff
+#define LLCP_MAX_MIU (LLCP_MAX_MIUX + 128)
+
+#define LLCP_WKS_NUM_SAP   16
+#define LLCP_SDP_NUM_SAP   16
+#define LLCP_LOCAL_NUM_SAP 32
+#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP)
+#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP)
+#define LLCP_SDP_UNBOUND   (LLCP_MAX_SAP + 1)
+
+struct nfc_llcp_sock;
+
+struct llcp_sock_list {
+       struct hlist_head head;
+       rwlock_t          lock;
+};
+
+struct nfc_llcp_sdp_tlv {
+       u8 *tlv;
+       u8 tlv_len;
+
+       char *uri;
+       u8 tid;
+       u8 sap;
+
+       unsigned long time;
+
+       struct hlist_node node;
+};
+
+struct nfc_llcp_local {
+       struct list_head list;
+       struct nfc_dev *dev;
+
+       struct kref ref;
+
+       struct mutex sdp_lock;
+
+       struct timer_list link_timer;
+       struct sk_buff_head tx_queue;
+       struct work_struct       tx_work;
+       struct work_struct       rx_work;
+       struct sk_buff *rx_pending;
+       struct work_struct       timeout_work;
+
+       u32 target_idx;
+       u8 rf_mode;
+       u8 comm_mode;
+       u8 lto;
+       u8 rw;
+       __be16 miux;
+       unsigned long local_wks;      /* Well known services */
+       unsigned long local_sdp;      /* Local services  */
+       unsigned long local_sap; /* Local SAPs, not available for discovery */
+       atomic_t local_sdp_cnt[LLCP_SDP_NUM_SAP];
+
+       /* local */
+       u8 gb[NFC_MAX_GT_LEN];
+       u8 gb_len;
+
+       /* remote */
+       u8 remote_gb[NFC_MAX_GT_LEN];
+       u8 remote_gb_len;
+
+       u8  remote_version;
+       u16 remote_miu;
+       u16 remote_lto;
+       u8  remote_opt;
+       u16 remote_wks;
+
+       struct mutex sdreq_lock;
+       struct hlist_head pending_sdreqs;
+       struct timer_list sdreq_timer;
+       struct work_struct sdreq_timeout_work;
+       u8 sdreq_next_tid;
+
+       /* sockets array */
+       struct llcp_sock_list sockets;
+       struct llcp_sock_list connecting_sockets;
+       struct llcp_sock_list raw_sockets;
+};
+
+struct nfc_llcp_sock {
+       struct sock sk;
+       struct nfc_dev *dev;
+       struct nfc_llcp_local *local;
+       u32 target_idx;
+       u32 nfc_protocol;
+
+       /* Link parameters */
+       u8 ssap;
+       u8 dsap;
+       char *service_name;
+       size_t service_name_len;
+       u8 rw;
+       __be16 miux;
+
+
+       /* Remote link parameters */
+       u8 remote_rw;
+       u16 remote_miu;
+
+       /* Link variables */
+       u8 send_n;
+       u8 send_ack_n;
+       u8 recv_n;
+       u8 recv_ack_n;
+
+       /* Is the remote peer ready to receive */
+       u8 remote_ready;
+
+       /* Reserved source SAP */
+       u8 reserved_ssap;
+
+       struct sk_buff_head tx_queue;
+       struct sk_buff_head tx_pending_queue;
+
+       struct list_head accept_queue;
+       struct sock *parent;
+};
+
+struct nfc_llcp_ui_cb {
+       __u8 dsap;
+       __u8 ssap;
+};
+
+#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0]))
+
+#define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
+#define nfc_llcp_dev(sk)  (nfc_llcp_sock((sk))->dev)
+
+#define LLCP_HEADER_SIZE   2
+#define LLCP_SEQUENCE_SIZE 1
+#define LLCP_AGF_PDU_HEADER_SIZE 2
+
+/* LLCP versions: 1.1 is 1.0 plus SDP */
+#define LLCP_VERSION_10 0x10
+#define LLCP_VERSION_11 0x11
+
+/* LLCP PDU types */
+#define LLCP_PDU_SYMM     0x0
+#define LLCP_PDU_PAX      0x1
+#define LLCP_PDU_AGF      0x2
+#define LLCP_PDU_UI       0x3
+#define LLCP_PDU_CONNECT  0x4
+#define LLCP_PDU_DISC     0x5
+#define LLCP_PDU_CC       0x6
+#define LLCP_PDU_DM       0x7
+#define LLCP_PDU_FRMR     0x8
+#define LLCP_PDU_SNL      0x9
+#define LLCP_PDU_I        0xc
+#define LLCP_PDU_RR       0xd
+#define LLCP_PDU_RNR      0xe
+
+/* Parameters TLV types */
+#define LLCP_TLV_VERSION 0x1
+#define LLCP_TLV_MIUX    0x2
+#define LLCP_TLV_WKS     0x3
+#define LLCP_TLV_LTO     0x4
+#define LLCP_TLV_RW      0x5
+#define LLCP_TLV_SN      0x6
+#define LLCP_TLV_OPT     0x7
+#define LLCP_TLV_SDREQ   0x8
+#define LLCP_TLV_SDRES   0x9
+#define LLCP_TLV_MAX     0xa
+
+/* Well known LLCP SAP */
+#define LLCP_SAP_SDP   0x1
+#define LLCP_SAP_IP    0x2
+#define LLCP_SAP_OBEX  0x3
+#define LLCP_SAP_SNEP  0x4
+#define LLCP_SAP_MAX   0xff
+
+/* Disconnection reason code */
+#define LLCP_DM_DISC    0x00
+#define LLCP_DM_NOCONN  0x01
+#define LLCP_DM_NOBOUND 0x02
+#define LLCP_DM_REJ     0x03
+
+
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *s);
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *s);
+void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock);
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local);
+int nfc_llcp_local_put(struct nfc_llcp_local *local);
+u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
+                        struct nfc_llcp_sock *sock);
+u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local);
+void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap);
+int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock);
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+                              struct sk_buff *skb, u8 direction);
+
+/* Sock API */
+struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);
+void nfc_llcp_sock_free(struct nfc_llcp_sock *sock);
+void nfc_llcp_accept_unlink(struct sock *sk);
+void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk);
+struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock);
+
+/* TLV API */
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+                         u8 *tlv_array, u16 tlv_array_len);
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+                                 u8 *tlv_array, u16 tlv_array_len);
+
+/* Commands API */
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
+u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length);
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap);
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+                                                 size_t uri_len);
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head);
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
+int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_symm(struct nfc_dev *dev);
+int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
+                           struct hlist_head *tlv_list, size_t tlvs_len);
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+                           struct hlist_head *tlv_list, size_t tlvs_len);
+int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
+int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
+int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
+                         struct msghdr *msg, size_t len);
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                          struct msghdr *msg, size_t len);
+int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
+
+/* Socket API */
+int __init nfc_llcp_sock_init(void);
+void nfc_llcp_sock_exit(void);
diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig
deleted file mode 100644 (file)
index a1a41cd..0000000
+++ /dev/null
@@ -1,7 +0,0 @@
-config NFC_LLCP
-       depends on NFC
-       bool "NFC LLCP support"
-       default n
-       help
-        Say Y here if you want to build support for a kernel NFC LLCP
-        implementation.
\ No newline at end of file
diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c
deleted file mode 100644 (file)
index 094f7e2..0000000
+++ /dev/null
@@ -1,817 +0,0 @@
-/*
- * Copyright (C) 2011  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 as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the
- * Free Software Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
-
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/nfc.h>
-
-#include <net/nfc/nfc.h>
-
-#include "../nfc.h"
-#include "llcp.h"
-
-static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
-       0,
-       1, /* VERSION */
-       2, /* MIUX */
-       2, /* WKS */
-       1, /* LTO */
-       1, /* RW */
-       0, /* SN */
-       1, /* OPT */
-       0, /* SDREQ */
-       2, /* SDRES */
-
-};
-
-static u8 llcp_tlv8(u8 *tlv, u8 type)
-{
-       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
-               return 0;
-
-       return tlv[2];
-}
-
-static u16 llcp_tlv16(u8 *tlv, u8 type)
-{
-       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
-               return 0;
-
-       return be16_to_cpu(*((__be16 *)(tlv + 2)));
-}
-
-
-static u8 llcp_tlv_version(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_VERSION);
-}
-
-static u16 llcp_tlv_miux(u8 *tlv)
-{
-       return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
-}
-
-static u16 llcp_tlv_wks(u8 *tlv)
-{
-       return llcp_tlv16(tlv, LLCP_TLV_WKS);
-}
-
-static u16 llcp_tlv_lto(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_LTO);
-}
-
-static u8 llcp_tlv_opt(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_OPT);
-}
-
-static u8 llcp_tlv_rw(u8 *tlv)
-{
-       return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
-}
-
-u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
-{
-       u8 *tlv, length;
-
-       pr_debug("type %d\n", type);
-
-       if (type >= LLCP_TLV_MAX)
-               return NULL;
-
-       length = llcp_tlv_length[type];
-       if (length == 0 && value_length == 0)
-               return NULL;
-       else if (length == 0)
-               length = value_length;
-
-       *tlv_length = 2 + length;
-       tlv = kzalloc(2 + length, GFP_KERNEL);
-       if (tlv == NULL)
-               return tlv;
-
-       tlv[0] = type;
-       tlv[1] = length;
-       memcpy(tlv + 2, value, length);
-
-       return tlv;
-}
-
-struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
-{
-       struct nfc_llcp_sdp_tlv *sdres;
-       u8 value[2];
-
-       sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
-       if (sdres == NULL)
-               return NULL;
-
-       value[0] = tid;
-       value[1] = sap;
-
-       sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
-                                       &sdres->tlv_len);
-       if (sdres->tlv == NULL) {
-               kfree(sdres);
-               return NULL;
-       }
-
-       sdres->tid = tid;
-       sdres->sap = sap;
-
-       INIT_HLIST_NODE(&sdres->node);
-
-       return sdres;
-}
-
-struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
-                                                 size_t uri_len)
-{
-       struct nfc_llcp_sdp_tlv *sdreq;
-
-       pr_debug("uri: %s, len: %zu\n", uri, uri_len);
-
-       sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
-       if (sdreq == NULL)
-               return NULL;
-
-       sdreq->tlv_len = uri_len + 3;
-
-       if (uri[uri_len - 1] == 0)
-               sdreq->tlv_len--;
-
-       sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
-       if (sdreq->tlv == NULL) {
-               kfree(sdreq);
-               return NULL;
-       }
-
-       sdreq->tlv[0] = LLCP_TLV_SDREQ;
-       sdreq->tlv[1] = sdreq->tlv_len - 2;
-       sdreq->tlv[2] = tid;
-
-       sdreq->tid = tid;
-       sdreq->uri = sdreq->tlv + 3;
-       memcpy(sdreq->uri, uri, uri_len);
-
-       sdreq->time = jiffies;
-
-       INIT_HLIST_NODE(&sdreq->node);
-
-       return sdreq;
-}
-
-void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
-{
-       kfree(sdp->tlv);
-       kfree(sdp);
-}
-
-void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
-{
-       struct nfc_llcp_sdp_tlv *sdp;
-       struct hlist_node *n;
-
-       hlist_for_each_entry_safe(sdp, n, head, node) {
-               hlist_del(&sdp->node);
-
-               nfc_llcp_free_sdp_tlv(sdp);
-       }
-}
-
-int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
-                         u8 *tlv_array, u16 tlv_array_len)
-{
-       u8 *tlv = tlv_array, type, length, offset = 0;
-
-       pr_debug("TLV array length %d\n", tlv_array_len);
-
-       if (local == NULL)
-               return -ENODEV;
-
-       while (offset < tlv_array_len) {
-               type = tlv[0];
-               length = tlv[1];
-
-               pr_debug("type 0x%x length %d\n", type, length);
-
-               switch (type) {
-               case LLCP_TLV_VERSION:
-                       local->remote_version = llcp_tlv_version(tlv);
-                       break;
-               case LLCP_TLV_MIUX:
-                       local->remote_miu = llcp_tlv_miux(tlv) + 128;
-                       break;
-               case LLCP_TLV_WKS:
-                       local->remote_wks = llcp_tlv_wks(tlv);
-                       break;
-               case LLCP_TLV_LTO:
-                       local->remote_lto = llcp_tlv_lto(tlv) * 10;
-                       break;
-               case LLCP_TLV_OPT:
-                       local->remote_opt = llcp_tlv_opt(tlv);
-                       break;
-               default:
-                       pr_err("Invalid gt tlv value 0x%x\n", type);
-                       break;
-               }
-
-               offset += length + 2;
-               tlv += length + 2;
-       }
-
-       pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
-                local->remote_version, local->remote_miu,
-                local->remote_lto, local->remote_opt,
-                local->remote_wks);
-
-       return 0;
-}
-
-int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
-                                 u8 *tlv_array, u16 tlv_array_len)
-{
-       u8 *tlv = tlv_array, type, length, offset = 0;
-
-       pr_debug("TLV array length %d\n", tlv_array_len);
-
-       if (sock == NULL)
-               return -ENOTCONN;
-
-       while (offset < tlv_array_len) {
-               type = tlv[0];
-               length = tlv[1];
-
-               pr_debug("type 0x%x length %d\n", type, length);
-
-               switch (type) {
-               case LLCP_TLV_MIUX:
-                       sock->remote_miu = llcp_tlv_miux(tlv) + 128;
-                       break;
-               case LLCP_TLV_RW:
-                       sock->remote_rw = llcp_tlv_rw(tlv);
-                       break;
-               case LLCP_TLV_SN:
-                       break;
-               default:
-                       pr_err("Invalid gt tlv value 0x%x\n", type);
-                       break;
-               }
-
-               offset += length + 2;
-               tlv += length + 2;
-       }
-
-       pr_debug("sock %p rw %d miu %d\n", sock,
-                sock->remote_rw, sock->remote_miu);
-
-       return 0;
-}
-
-static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
-                                      u8 dsap, u8 ssap, u8 ptype)
-{
-       u8 header[2];
-
-       pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
-
-       header[0] = (u8)((dsap << 2) | (ptype >> 2));
-       header[1] = (u8)((ptype << 6) | ssap);
-
-       pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
-
-       memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
-
-       return pdu;
-}
-
-static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv,
-                                   u8 tlv_length)
-{
-       /* XXX Add an skb length check */
-
-       if (tlv == NULL)
-               return NULL;
-
-       memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
-
-       return pdu;
-}
-
-static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
-                                        u8 cmd, u16 size)
-{
-       struct sk_buff *skb;
-       int err;
-
-       if (sock->ssap == 0)
-               return NULL;
-
-       skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
-                                size + LLCP_HEADER_SIZE, &err);
-       if (skb == NULL) {
-               pr_err("Could not allocate PDU\n");
-               return NULL;
-       }
-
-       skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
-
-       return skb;
-}
-
-int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
-{
-       struct sk_buff *skb;
-       struct nfc_dev *dev;
-       struct nfc_llcp_local *local;
-
-       pr_debug("Sending DISC\n");
-
-       local = sock->local;
-       if (local == NULL)
-               return -ENODEV;
-
-       dev = sock->dev;
-       if (dev == NULL)
-               return -ENODEV;
-
-       skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
-       if (skb == NULL)
-               return -ENOMEM;
-
-       skb_queue_tail(&local->tx_queue, skb);
-
-       return 0;
-}
-
-int nfc_llcp_send_symm(struct nfc_dev *dev)
-{
-       struct sk_buff *skb;
-       struct nfc_llcp_local *local;
-       u16 size = 0;
-
-       pr_debug("Sending SYMM\n");
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL)
-               return -ENODEV;
-
-       size += LLCP_HEADER_SIZE;
-       size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
-
-       skb = alloc_skb(size, GFP_KERNEL);
-       if (skb == NULL)
-               return -ENOMEM;
-
-       skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
-
-       skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
-
-       __net_timestamp(skb);
-
-       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX);
-
-       return nfc_data_exchange(dev, local->target_idx, skb,
-                                nfc_llcp_recv, local);
-}
-
-int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
-{
-       struct nfc_llcp_local *local;
-       struct sk_buff *skb;
-       u8 *service_name_tlv = NULL, service_name_tlv_length;
-       u8 *miux_tlv = NULL, miux_tlv_length;
-       u8 *rw_tlv = NULL, rw_tlv_length, rw;
-       int err;
-       u16 size = 0, miux;
-
-       pr_debug("Sending CONNECT\n");
-
-       local = sock->local;
-       if (local == NULL)
-               return -ENODEV;
-
-       if (sock->service_name != NULL) {
-               service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN,
-                                                     sock->service_name,
-                                                     sock->service_name_len,
-                                                     &service_name_tlv_length);
-               size += service_name_tlv_length;
-       }
-
-       /* If the socket parameters are not set, use the local ones */
-       miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
-               local->miux : sock->miux;
-       rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
-
-       miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
-                                     &miux_tlv_length);
-       size += miux_tlv_length;
-
-       rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
-       size += rw_tlv_length;
-
-       pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
-
-       skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size);
-       if (skb == NULL) {
-               err = -ENOMEM;
-               goto error_tlv;
-       }
-
-       if (service_name_tlv != NULL)
-               skb = llcp_add_tlv(skb, service_name_tlv,
-                                  service_name_tlv_length);
-
-       skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
-       skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
-
-       skb_queue_tail(&local->tx_queue, skb);
-
-       return 0;
-
-error_tlv:
-       pr_err("error %d\n", err);
-
-       kfree(service_name_tlv);
-       kfree(miux_tlv);
-       kfree(rw_tlv);
-
-       return err;
-}
-
-int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
-{
-       struct nfc_llcp_local *local;
-       struct sk_buff *skb;
-       u8 *miux_tlv = NULL, miux_tlv_length;
-       u8 *rw_tlv = NULL, rw_tlv_length, rw;
-       int err;
-       u16 size = 0, miux;
-
-       pr_debug("Sending CC\n");
-
-       local = sock->local;
-       if (local == NULL)
-               return -ENODEV;
-
-       /* If the socket parameters are not set, use the local ones */
-       miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
-               local->miux : sock->miux;
-       rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
-
-       miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
-                                     &miux_tlv_length);
-       size += miux_tlv_length;
-
-       rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
-       size += rw_tlv_length;
-
-       skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
-       if (skb == NULL) {
-               err = -ENOMEM;
-               goto error_tlv;
-       }
-
-       skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
-       skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
-
-       skb_queue_tail(&local->tx_queue, skb);
-
-       return 0;
-
-error_tlv:
-       pr_err("error %d\n", err);
-
-       kfree(miux_tlv);
-       kfree(rw_tlv);
-
-       return err;
-}
-
-static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,
-                                            size_t tlv_length)
-{
-       struct sk_buff *skb;
-       struct nfc_dev *dev;
-       u16 size = 0;
-
-       if (local == NULL)
-               return ERR_PTR(-ENODEV);
-
-       dev = local->dev;
-       if (dev == NULL)
-               return ERR_PTR(-ENODEV);
-
-       size += LLCP_HEADER_SIZE;
-       size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
-       size += tlv_length;
-
-       skb = alloc_skb(size, GFP_KERNEL);
-       if (skb == NULL)
-               return ERR_PTR(-ENOMEM);
-
-       skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
-
-       skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
-
-       return skb;
-}
-
-int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
-                           struct hlist_head *tlv_list, size_t tlvs_len)
-{
-       struct nfc_llcp_sdp_tlv *sdp;
-       struct hlist_node *n;
-       struct sk_buff *skb;
-
-       skb = nfc_llcp_allocate_snl(local, tlvs_len);
-       if (IS_ERR(skb))
-               return PTR_ERR(skb);
-
-       hlist_for_each_entry_safe(sdp, n, tlv_list, node) {
-               memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len);
-
-               hlist_del(&sdp->node);
-
-               nfc_llcp_free_sdp_tlv(sdp);
-       }
-
-       skb_queue_tail(&local->tx_queue, skb);
-
-       return 0;
-}
-
-int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
-                           struct hlist_head *tlv_list, size_t tlvs_len)
-{
-       struct nfc_llcp_sdp_tlv *sdreq;
-       struct hlist_node *n;
-       struct sk_buff *skb;
-
-       skb = nfc_llcp_allocate_snl(local, tlvs_len);
-       if (IS_ERR(skb))
-               return PTR_ERR(skb);
-
-       mutex_lock(&local->sdreq_lock);
-
-       if (hlist_empty(&local->pending_sdreqs))
-               mod_timer(&local->sdreq_timer,
-                         jiffies + msecs_to_jiffies(3 * local->remote_lto));
-
-       hlist_for_each_entry_safe(sdreq, n, tlv_list, node) {
-               pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri);
-
-               memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv,
-                      sdreq->tlv_len);
-
-               hlist_del(&sdreq->node);
-
-               hlist_add_head(&sdreq->node, &local->pending_sdreqs);
-       }
-
-       mutex_unlock(&local->sdreq_lock);
-
-       skb_queue_tail(&local->tx_queue, skb);
-
-       return 0;
-}
-
-int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
-{
-       struct sk_buff *skb;
-       struct nfc_dev *dev;
-       u16 size = 1; /* Reason code */
-
-       pr_debug("Sending DM reason 0x%x\n", reason);
-
-       if (local == NULL)
-               return -ENODEV;
-
-       dev = local->dev;
-       if (dev == NULL)
-               return -ENODEV;
-
-       size += LLCP_HEADER_SIZE;
-       size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
-
-       skb = alloc_skb(size, GFP_KERNEL);
-       if (skb == NULL)
-               return -ENOMEM;
-
-       skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
-
-       skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM);
-
-       memcpy(skb_put(skb, 1), &reason, 1);
-
-       skb_queue_head(&local->tx_queue, skb);
-
-       return 0;
-}
-
-int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock)
-{
-       struct sk_buff *skb;
-       struct nfc_llcp_local *local;
-
-       pr_debug("Send DISC\n");
-
-       local = sock->local;
-       if (local == NULL)
-               return -ENODEV;
-
-       skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
-       if (skb == NULL)
-               return -ENOMEM;
-
-       skb_queue_head(&local->tx_queue, skb);
-
-       return 0;
-}
-
-int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
-                         struct msghdr *msg, size_t len)
-{
-       struct sk_buff *pdu;
-       struct sock *sk = &sock->sk;
-       struct nfc_llcp_local *local;
-       size_t frag_len = 0, remaining_len;
-       u8 *msg_data, *msg_ptr;
-       u16 remote_miu;
-
-       pr_debug("Send I frame len %zd\n", len);
-
-       local = sock->local;
-       if (local == NULL)
-               return -ENODEV;
-
-       /* Remote is ready but has not acknowledged our frames */
-       if((sock->remote_ready &&
-           skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw &&
-           skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
-               pr_err("Pending queue is full %d frames\n",
-                      skb_queue_len(&sock->tx_pending_queue));
-               return -ENOBUFS;
-       }
-
-       /* Remote is not ready and we've been queueing enough frames */
-       if ((!sock->remote_ready &&
-            skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
-               pr_err("Tx queue is full %d frames\n",
-                      skb_queue_len(&sock->tx_queue));
-               return -ENOBUFS;
-       }
-
-       msg_data = kzalloc(len, GFP_KERNEL);
-       if (msg_data == NULL)
-               return -ENOMEM;
-
-       if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
-               kfree(msg_data);
-               return -EFAULT;
-       }
-
-       remaining_len = len;
-       msg_ptr = msg_data;
-
-       do {
-               remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
-                               local->remote_miu : sock->remote_miu;
-
-               frag_len = min_t(size_t, remote_miu, remaining_len);
-
-               pr_debug("Fragment %zd bytes remaining %zd",
-                        frag_len, remaining_len);
-
-               pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
-                                       frag_len + LLCP_SEQUENCE_SIZE);
-               if (pdu == NULL)
-                       return -ENOMEM;
-
-               skb_put(pdu, LLCP_SEQUENCE_SIZE);
-
-               if (likely(frag_len > 0))
-                       memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
-
-               skb_queue_tail(&sock->tx_queue, pdu);
-
-               lock_sock(sk);
-
-               nfc_llcp_queue_i_frames(sock);
-
-               release_sock(sk);
-
-               remaining_len -= frag_len;
-               msg_ptr += frag_len;
-       } while (remaining_len > 0);
-
-       kfree(msg_data);
-
-       return len;
-}
-
-int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
-                          struct msghdr *msg, size_t len)
-{
-       struct sk_buff *pdu;
-       struct nfc_llcp_local *local;
-       size_t frag_len = 0, remaining_len;
-       u8 *msg_ptr, *msg_data;
-       u16 remote_miu;
-       int err;
-
-       pr_debug("Send UI frame len %zd\n", len);
-
-       local = sock->local;
-       if (local == NULL)
-               return -ENODEV;
-
-       msg_data = kzalloc(len, GFP_KERNEL);
-       if (msg_data == NULL)
-               return -ENOMEM;
-
-       if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
-               kfree(msg_data);
-               return -EFAULT;
-       }
-
-       remaining_len = len;
-       msg_ptr = msg_data;
-
-       do {
-               remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
-                               local->remote_miu : sock->remote_miu;
-
-               frag_len = min_t(size_t, remote_miu, remaining_len);
-
-               pr_debug("Fragment %zd bytes remaining %zd",
-                        frag_len, remaining_len);
-
-               pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
-                                        frag_len + LLCP_HEADER_SIZE, &err);
-               if (pdu == NULL) {
-                       pr_err("Could not allocate PDU\n");
-                       continue;
-               }
-
-               pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
-
-               if (likely(frag_len > 0))
-                       memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
-
-               /* No need to check for the peer RW for UI frames */
-               skb_queue_tail(&local->tx_queue, pdu);
-
-               remaining_len -= frag_len;
-               msg_ptr += frag_len;
-       } while (remaining_len > 0);
-
-       kfree(msg_data);
-
-       return len;
-}
-
-int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
-{
-       struct sk_buff *skb;
-       struct nfc_llcp_local *local;
-
-       pr_debug("Send rr nr %d\n", sock->recv_n);
-
-       local = sock->local;
-       if (local == NULL)
-               return -ENODEV;
-
-       skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE);
-       if (skb == NULL)
-               return -ENOMEM;
-
-       skb_put(skb, LLCP_SEQUENCE_SIZE);
-
-       skb->data[2] = sock->recv_n;
-
-       skb_queue_head(&local->tx_queue, skb);
-
-       return 0;
-}
diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
deleted file mode 100644 (file)
index 9e483c8..0000000
+++ /dev/null
@@ -1,1624 +0,0 @@
-/*
- * Copyright (C) 2011  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 as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the
- * Free Software Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
-
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/list.h>
-#include <linux/nfc.h>
-
-#include "../nfc.h"
-#include "llcp.h"
-
-static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
-
-static struct list_head llcp_devices;
-
-static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
-
-void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
-{
-       write_lock(&l->lock);
-       sk_add_node(sk, &l->head);
-       write_unlock(&l->lock);
-}
-
-void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
-{
-       write_lock(&l->lock);
-       sk_del_node_init(sk);
-       write_unlock(&l->lock);
-}
-
-void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
-{
-       sock->remote_rw = LLCP_DEFAULT_RW;
-       sock->remote_miu = LLCP_MAX_MIU + 1;
-}
-
-static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
-{
-       struct nfc_llcp_local *local = sock->local;
-       struct sk_buff *s, *tmp;
-
-       pr_debug("%p\n", &sock->sk);
-
-       skb_queue_purge(&sock->tx_queue);
-       skb_queue_purge(&sock->tx_pending_queue);
-
-       if (local == NULL)
-               return;
-
-       /* Search for local pending SKBs that are related to this socket */
-       skb_queue_walk_safe(&local->tx_queue, s, tmp) {
-               if (s->sk != &sock->sk)
-                       continue;
-
-               skb_unlink(s, &local->tx_queue);
-               kfree_skb(s);
-       }
-}
-
-static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
-                                   int err)
-{
-       struct sock *sk;
-       struct hlist_node *tmp;
-       struct nfc_llcp_sock *llcp_sock;
-
-       skb_queue_purge(&local->tx_queue);
-
-       write_lock(&local->sockets.lock);
-
-       sk_for_each_safe(sk, tmp, &local->sockets.head) {
-               llcp_sock = nfc_llcp_sock(sk);
-
-               bh_lock_sock(sk);
-
-               nfc_llcp_socket_purge(llcp_sock);
-
-               if (sk->sk_state == LLCP_CONNECTED)
-                       nfc_put_device(llcp_sock->dev);
-
-               if (sk->sk_state == LLCP_LISTEN) {
-                       struct nfc_llcp_sock *lsk, *n;
-                       struct sock *accept_sk;
-
-                       list_for_each_entry_safe(lsk, n,
-                                                &llcp_sock->accept_queue,
-                                                accept_queue) {
-                               accept_sk = &lsk->sk;
-                               bh_lock_sock(accept_sk);
-
-                               nfc_llcp_accept_unlink(accept_sk);
-
-                               if (err)
-                                       accept_sk->sk_err = err;
-                               accept_sk->sk_state = LLCP_CLOSED;
-                               accept_sk->sk_state_change(sk);
-
-                               bh_unlock_sock(accept_sk);
-                       }
-               }
-
-               if (err)
-                       sk->sk_err = err;
-               sk->sk_state = LLCP_CLOSED;
-               sk->sk_state_change(sk);
-
-               bh_unlock_sock(sk);
-
-               sk_del_node_init(sk);
-       }
-
-       write_unlock(&local->sockets.lock);
-
-       /* If we still have a device, we keep the RAW sockets alive */
-       if (device == true)
-               return;
-
-       write_lock(&local->raw_sockets.lock);
-
-       sk_for_each_safe(sk, tmp, &local->raw_sockets.head) {
-               llcp_sock = nfc_llcp_sock(sk);
-
-               bh_lock_sock(sk);
-
-               nfc_llcp_socket_purge(llcp_sock);
-
-               if (err)
-                       sk->sk_err = err;
-               sk->sk_state = LLCP_CLOSED;
-               sk->sk_state_change(sk);
-
-               bh_unlock_sock(sk);
-
-               sk_del_node_init(sk);
-       }
-
-       write_unlock(&local->raw_sockets.lock);
-}
-
-struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
-{
-       kref_get(&local->ref);
-
-       return local;
-}
-
-static void local_cleanup(struct nfc_llcp_local *local)
-{
-       nfc_llcp_socket_release(local, false, ENXIO);
-       del_timer_sync(&local->link_timer);
-       skb_queue_purge(&local->tx_queue);
-       cancel_work_sync(&local->tx_work);
-       cancel_work_sync(&local->rx_work);
-       cancel_work_sync(&local->timeout_work);
-       kfree_skb(local->rx_pending);
-       del_timer_sync(&local->sdreq_timer);
-       cancel_work_sync(&local->sdreq_timeout_work);
-       nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
-}
-
-static void local_release(struct kref *ref)
-{
-       struct nfc_llcp_local *local;
-
-       local = container_of(ref, struct nfc_llcp_local, ref);
-
-       list_del(&local->list);
-       local_cleanup(local);
-       kfree(local);
-}
-
-int nfc_llcp_local_put(struct nfc_llcp_local *local)
-{
-       if (local == NULL)
-               return 0;
-
-       return kref_put(&local->ref, local_release);
-}
-
-static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
-                                              u8 ssap, u8 dsap)
-{
-       struct sock *sk;
-       struct nfc_llcp_sock *llcp_sock, *tmp_sock;
-
-       pr_debug("ssap dsap %d %d\n", ssap, dsap);
-
-       if (ssap == 0 && dsap == 0)
-               return NULL;
-
-       read_lock(&local->sockets.lock);
-
-       llcp_sock = NULL;
-
-       sk_for_each(sk, &local->sockets.head) {
-               tmp_sock = nfc_llcp_sock(sk);
-
-               if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
-                       llcp_sock = tmp_sock;
-                       break;
-               }
-       }
-
-       read_unlock(&local->sockets.lock);
-
-       if (llcp_sock == NULL)
-               return NULL;
-
-       sock_hold(&llcp_sock->sk);
-
-       return llcp_sock;
-}
-
-static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
-{
-       sock_put(&sock->sk);
-}
-
-static void nfc_llcp_timeout_work(struct work_struct *work)
-{
-       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                   timeout_work);
-
-       nfc_dep_link_down(local->dev);
-}
-
-static void nfc_llcp_symm_timer(unsigned long data)
-{
-       struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
-
-       pr_err("SYMM timeout\n");
-
-       schedule_work(&local->timeout_work);
-}
-
-static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
-{
-       unsigned long time;
-       HLIST_HEAD(nl_sdres_list);
-       struct hlist_node *n;
-       struct nfc_llcp_sdp_tlv *sdp;
-       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                   sdreq_timeout_work);
-
-       mutex_lock(&local->sdreq_lock);
-
-       time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
-
-       hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
-               if (time_after(sdp->time, time))
-                       continue;
-
-               sdp->sap = LLCP_SDP_UNBOUND;
-
-               hlist_del(&sdp->node);
-
-               hlist_add_head(&sdp->node, &nl_sdres_list);
-       }
-
-       if (!hlist_empty(&local->pending_sdreqs))
-               mod_timer(&local->sdreq_timer,
-                         jiffies + msecs_to_jiffies(3 * local->remote_lto));
-
-       mutex_unlock(&local->sdreq_lock);
-
-       if (!hlist_empty(&nl_sdres_list))
-               nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
-}
-
-static void nfc_llcp_sdreq_timer(unsigned long data)
-{
-       struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
-
-       schedule_work(&local->sdreq_timeout_work);
-}
-
-struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
-{
-       struct nfc_llcp_local *local, *n;
-
-       list_for_each_entry_safe(local, n, &llcp_devices, list)
-               if (local->dev == dev)
-                       return local;
-
-       pr_debug("No device found\n");
-
-       return NULL;
-}
-
-static char *wks[] = {
-       NULL,
-       NULL, /* SDP */
-       "urn:nfc:sn:ip",
-       "urn:nfc:sn:obex",
-       "urn:nfc:sn:snep",
-};
-
-static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
-{
-       int sap, num_wks;
-
-       pr_debug("%s\n", service_name);
-
-       if (service_name == NULL)
-               return -EINVAL;
-
-       num_wks = ARRAY_SIZE(wks);
-
-       for (sap = 0; sap < num_wks; sap++) {
-               if (wks[sap] == NULL)
-                       continue;
-
-               if (strncmp(wks[sap], service_name, service_name_len) == 0)
-                       return sap;
-       }
-
-       return -EINVAL;
-}
-
-static
-struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
-                                           u8 *sn, size_t sn_len)
-{
-       struct sock *sk;
-       struct nfc_llcp_sock *llcp_sock, *tmp_sock;
-
-       pr_debug("sn %zd %p\n", sn_len, sn);
-
-       if (sn == NULL || sn_len == 0)
-               return NULL;
-
-       read_lock(&local->sockets.lock);
-
-       llcp_sock = NULL;
-
-       sk_for_each(sk, &local->sockets.head) {
-               tmp_sock = nfc_llcp_sock(sk);
-
-               pr_debug("llcp sock %p\n", tmp_sock);
-
-               if (tmp_sock->sk.sk_type == SOCK_STREAM &&
-                   tmp_sock->sk.sk_state != LLCP_LISTEN)
-                       continue;
-
-               if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
-                   tmp_sock->sk.sk_state != LLCP_BOUND)
-                       continue;
-
-               if (tmp_sock->service_name == NULL ||
-                   tmp_sock->service_name_len == 0)
-                       continue;
-
-               if (tmp_sock->service_name_len != sn_len)
-                       continue;
-
-               if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
-                       llcp_sock = tmp_sock;
-                       break;
-               }
-       }
-
-       read_unlock(&local->sockets.lock);
-
-       pr_debug("Found llcp sock %p\n", llcp_sock);
-
-       return llcp_sock;
-}
-
-u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
-                        struct nfc_llcp_sock *sock)
-{
-       mutex_lock(&local->sdp_lock);
-
-       if (sock->service_name != NULL && sock->service_name_len > 0) {
-               int ssap = nfc_llcp_wks_sap(sock->service_name,
-                                           sock->service_name_len);
-
-               if (ssap > 0) {
-                       pr_debug("WKS %d\n", ssap);
-
-                       /* This is a WKS, let's check if it's free */
-                       if (local->local_wks & BIT(ssap)) {
-                               mutex_unlock(&local->sdp_lock);
-
-                               return LLCP_SAP_MAX;
-                       }
-
-                       set_bit(ssap, &local->local_wks);
-                       mutex_unlock(&local->sdp_lock);
-
-                       return ssap;
-               }
-
-               /*
-                * Check if there already is a non WKS socket bound
-                * to this service name.
-                */
-               if (nfc_llcp_sock_from_sn(local, sock->service_name,
-                                         sock->service_name_len) != NULL) {
-                       mutex_unlock(&local->sdp_lock);
-
-                       return LLCP_SAP_MAX;
-               }
-
-               mutex_unlock(&local->sdp_lock);
-
-               return LLCP_SDP_UNBOUND;
-
-       } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
-               if (!test_bit(sock->ssap, &local->local_wks)) {
-                       set_bit(sock->ssap, &local->local_wks);
-                       mutex_unlock(&local->sdp_lock);
-
-                       return sock->ssap;
-               }
-       }
-
-       mutex_unlock(&local->sdp_lock);
-
-       return LLCP_SAP_MAX;
-}
-
-u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
-{
-       u8 local_ssap;
-
-       mutex_lock(&local->sdp_lock);
-
-       local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
-       if (local_ssap == LLCP_LOCAL_NUM_SAP) {
-               mutex_unlock(&local->sdp_lock);
-               return LLCP_SAP_MAX;
-       }
-
-       set_bit(local_ssap, &local->local_sap);
-
-       mutex_unlock(&local->sdp_lock);
-
-       return local_ssap + LLCP_LOCAL_SAP_OFFSET;
-}
-
-void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
-{
-       u8 local_ssap;
-       unsigned long *sdp;
-
-       if (ssap < LLCP_WKS_NUM_SAP) {
-               local_ssap = ssap;
-               sdp = &local->local_wks;
-       } else if (ssap < LLCP_LOCAL_NUM_SAP) {
-               atomic_t *client_cnt;
-
-               local_ssap = ssap - LLCP_WKS_NUM_SAP;
-               sdp = &local->local_sdp;
-               client_cnt = &local->local_sdp_cnt[local_ssap];
-
-               pr_debug("%d clients\n", atomic_read(client_cnt));
-
-               mutex_lock(&local->sdp_lock);
-
-               if (atomic_dec_and_test(client_cnt)) {
-                       struct nfc_llcp_sock *l_sock;
-
-                       pr_debug("No more clients for SAP %d\n", ssap);
-
-                       clear_bit(local_ssap, sdp);
-
-                       /* Find the listening sock and set it back to UNBOUND */
-                       l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
-                       if (l_sock) {
-                               l_sock->ssap = LLCP_SDP_UNBOUND;
-                               nfc_llcp_sock_put(l_sock);
-                       }
-               }
-
-               mutex_unlock(&local->sdp_lock);
-
-               return;
-       } else if (ssap < LLCP_MAX_SAP) {
-               local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
-               sdp = &local->local_sap;
-       } else {
-               return;
-       }
-
-       mutex_lock(&local->sdp_lock);
-
-       clear_bit(local_ssap, sdp);
-
-       mutex_unlock(&local->sdp_lock);
-}
-
-static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
-{
-       u8 ssap;
-
-       mutex_lock(&local->sdp_lock);
-
-       ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
-       if (ssap == LLCP_SDP_NUM_SAP) {
-               mutex_unlock(&local->sdp_lock);
-
-               return LLCP_SAP_MAX;
-       }
-
-       pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
-
-       set_bit(ssap, &local->local_sdp);
-
-       mutex_unlock(&local->sdp_lock);
-
-       return LLCP_WKS_NUM_SAP + ssap;
-}
-
-static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
-{
-       u8 *gb_cur, *version_tlv, version, version_length;
-       u8 *lto_tlv, lto_length;
-       u8 *wks_tlv, wks_length;
-       u8 *miux_tlv, miux_length;
-       u8 gb_len = 0;
-       int ret = 0;
-
-       version = LLCP_VERSION_11;
-       version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
-                                        1, &version_length);
-       gb_len += version_length;
-
-       lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
-       gb_len += lto_length;
-
-       pr_debug("Local wks 0x%lx\n", local->local_wks);
-       wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
-                                    &wks_length);
-       gb_len += wks_length;
-
-       miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
-                                     &miux_length);
-       gb_len += miux_length;
-
-       gb_len += ARRAY_SIZE(llcp_magic);
-
-       if (gb_len > NFC_MAX_GT_LEN) {
-               ret = -EINVAL;
-               goto out;
-       }
-
-       gb_cur = local->gb;
-
-       memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
-       gb_cur += ARRAY_SIZE(llcp_magic);
-
-       memcpy(gb_cur, version_tlv, version_length);
-       gb_cur += version_length;
-
-       memcpy(gb_cur, lto_tlv, lto_length);
-       gb_cur += lto_length;
-
-       memcpy(gb_cur, wks_tlv, wks_length);
-       gb_cur += wks_length;
-
-       memcpy(gb_cur, miux_tlv, miux_length);
-       gb_cur += miux_length;
-
-       local->gb_len = gb_len;
-
-out:
-       kfree(version_tlv);
-       kfree(lto_tlv);
-       kfree(wks_tlv);
-       kfree(miux_tlv);
-
-       return ret;
-}
-
-u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
-{
-       struct nfc_llcp_local *local;
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL) {
-               *general_bytes_len = 0;
-               return NULL;
-       }
-
-       nfc_llcp_build_gb(local);
-
-       *general_bytes_len = local->gb_len;
-
-       return local->gb;
-}
-
-int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
-{
-       struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
-
-       if (local == NULL) {
-               pr_err("No LLCP device\n");
-               return -ENODEV;
-       }
-       if (gb_len < 3)
-               return -EINVAL;
-
-       memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
-       memcpy(local->remote_gb, gb, gb_len);
-       local->remote_gb_len = gb_len;
-
-       if (memcmp(local->remote_gb, llcp_magic, 3)) {
-               pr_err("MAC does not support LLCP\n");
-               return -EINVAL;
-       }
-
-       return nfc_llcp_parse_gb_tlv(local,
-                                    &local->remote_gb[3],
-                                    local->remote_gb_len - 3);
-}
-
-static u8 nfc_llcp_dsap(struct sk_buff *pdu)
-{
-       return (pdu->data[0] & 0xfc) >> 2;
-}
-
-static u8 nfc_llcp_ptype(struct sk_buff *pdu)
-{
-       return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
-}
-
-static u8 nfc_llcp_ssap(struct sk_buff *pdu)
-{
-       return pdu->data[1] & 0x3f;
-}
-
-static u8 nfc_llcp_ns(struct sk_buff *pdu)
-{
-       return pdu->data[2] >> 4;
-}
-
-static u8 nfc_llcp_nr(struct sk_buff *pdu)
-{
-       return pdu->data[2] & 0xf;
-}
-
-static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
-{
-       pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
-       sock->send_n = (sock->send_n + 1) % 16;
-       sock->recv_ack_n = (sock->recv_n - 1) % 16;
-}
-
-void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
-                              struct sk_buff *skb, u8 direction)
-{
-       struct sk_buff *skb_copy = NULL, *nskb;
-       struct sock *sk;
-       u8 *data;
-
-       read_lock(&local->raw_sockets.lock);
-
-       sk_for_each(sk, &local->raw_sockets.head) {
-               if (sk->sk_state != LLCP_BOUND)
-                       continue;
-
-               if (skb_copy == NULL) {
-                       skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
-                                              GFP_ATOMIC);
-
-                       if (skb_copy == NULL)
-                               continue;
-
-                       data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
-
-                       data[0] = local->dev ? local->dev->idx : 0xFF;
-                       data[1] = direction;
-               }
-
-               nskb = skb_clone(skb_copy, GFP_ATOMIC);
-               if (!nskb)
-                       continue;
-
-               if (sock_queue_rcv_skb(sk, nskb))
-                       kfree_skb(nskb);
-       }
-
-       read_unlock(&local->raw_sockets.lock);
-
-       kfree_skb(skb_copy);
-}
-
-static void nfc_llcp_tx_work(struct work_struct *work)
-{
-       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                   tx_work);
-       struct sk_buff *skb;
-       struct sock *sk;
-       struct nfc_llcp_sock *llcp_sock;
-
-       skb = skb_dequeue(&local->tx_queue);
-       if (skb != NULL) {
-               sk = skb->sk;
-               llcp_sock = nfc_llcp_sock(sk);
-
-               if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
-                       nfc_llcp_send_symm(local->dev);
-               } else {
-                       struct sk_buff *copy_skb = NULL;
-                       u8 ptype = nfc_llcp_ptype(skb);
-                       int ret;
-
-                       pr_debug("Sending pending skb\n");
-                       print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
-                                      DUMP_PREFIX_OFFSET, 16, 1,
-                                      skb->data, skb->len, true);
-
-                       if (ptype == LLCP_PDU_I)
-                               copy_skb = skb_copy(skb, GFP_ATOMIC);
-
-                       __net_timestamp(skb);
-
-                       nfc_llcp_send_to_raw_sock(local, skb,
-                                                 NFC_LLCP_DIRECTION_TX);
-
-                       ret = nfc_data_exchange(local->dev, local->target_idx,
-                                               skb, nfc_llcp_recv, local);
-
-                       if (ret) {
-                               kfree_skb(copy_skb);
-                               goto out;
-                       }
-
-                       if (ptype == LLCP_PDU_I && copy_skb)
-                               skb_queue_tail(&llcp_sock->tx_pending_queue,
-                                              copy_skb);
-               }
-       } else {
-               nfc_llcp_send_symm(local->dev);
-       }
-
-out:
-       mod_timer(&local->link_timer,
-                 jiffies + msecs_to_jiffies(2 * local->remote_lto));
-}
-
-static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
-                                                         u8 ssap)
-{
-       struct sock *sk;
-       struct nfc_llcp_sock *llcp_sock;
-
-       read_lock(&local->connecting_sockets.lock);
-
-       sk_for_each(sk, &local->connecting_sockets.head) {
-               llcp_sock = nfc_llcp_sock(sk);
-
-               if (llcp_sock->ssap == ssap) {
-                       sock_hold(&llcp_sock->sk);
-                       goto out;
-               }
-       }
-
-       llcp_sock = NULL;
-
-out:
-       read_unlock(&local->connecting_sockets.lock);
-
-       return llcp_sock;
-}
-
-static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
-                                                 u8 *sn, size_t sn_len)
-{
-       struct nfc_llcp_sock *llcp_sock;
-
-       llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
-
-       if (llcp_sock == NULL)
-               return NULL;
-
-       sock_hold(&llcp_sock->sk);
-
-       return llcp_sock;
-}
-
-static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
-{
-       u8 *tlv = &skb->data[2], type, length;
-       size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
-
-       while (offset < tlv_array_len) {
-               type = tlv[0];
-               length = tlv[1];
-
-               pr_debug("type 0x%x length %d\n", type, length);
-
-               if (type == LLCP_TLV_SN) {
-                       *sn_len = length;
-                       return &tlv[2];
-               }
-
-               offset += length + 2;
-               tlv += length + 2;
-       }
-
-       return NULL;
-}
-
-static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
-                            struct sk_buff *skb)
-{
-       struct nfc_llcp_sock *llcp_sock;
-       struct nfc_llcp_ui_cb *ui_cb;
-       u8 dsap, ssap;
-
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-
-       ui_cb = nfc_llcp_ui_skb_cb(skb);
-       ui_cb->dsap = dsap;
-       ui_cb->ssap = ssap;
-
-       pr_debug("%d %d\n", dsap, ssap);
-
-       /* We're looking for a bound socket, not a client one */
-       llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
-       if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
-               return;
-
-       /* There is no sequence with UI frames */
-       skb_pull(skb, LLCP_HEADER_SIZE);
-       if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
-               /*
-                * UI frames will be freed from the socket layer, so we
-                * need to keep them alive until someone receives them.
-                */
-               skb_get(skb);
-       } else {
-               pr_err("Receive queue is full\n");
-       }
-
-       nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
-                                 struct sk_buff *skb)
-{
-       struct sock *new_sk, *parent;
-       struct nfc_llcp_sock *sock, *new_sock;
-       u8 dsap, ssap, reason;
-
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-
-       pr_debug("%d %d\n", dsap, ssap);
-
-       if (dsap != LLCP_SAP_SDP) {
-               sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
-               if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
-                       reason = LLCP_DM_NOBOUND;
-                       goto fail;
-               }
-       } else {
-               u8 *sn;
-               size_t sn_len;
-
-               sn = nfc_llcp_connect_sn(skb, &sn_len);
-               if (sn == NULL) {
-                       reason = LLCP_DM_NOBOUND;
-                       goto fail;
-               }
-
-               pr_debug("Service name length %zu\n", sn_len);
-
-               sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
-               if (sock == NULL) {
-                       reason = LLCP_DM_NOBOUND;
-                       goto fail;
-               }
-       }
-
-       lock_sock(&sock->sk);
-
-       parent = &sock->sk;
-
-       if (sk_acceptq_is_full(parent)) {
-               reason = LLCP_DM_REJ;
-               release_sock(&sock->sk);
-               sock_put(&sock->sk);
-               goto fail;
-       }
-
-       if (sock->ssap == LLCP_SDP_UNBOUND) {
-               u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
-
-               pr_debug("First client, reserving %d\n", ssap);
-
-               if (ssap == LLCP_SAP_MAX) {
-                       reason = LLCP_DM_REJ;
-                       release_sock(&sock->sk);
-                       sock_put(&sock->sk);
-                       goto fail;
-               }
-
-               sock->ssap = ssap;
-       }
-
-       new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
-       if (new_sk == NULL) {
-               reason = LLCP_DM_REJ;
-               release_sock(&sock->sk);
-               sock_put(&sock->sk);
-               goto fail;
-       }
-
-       new_sock = nfc_llcp_sock(new_sk);
-       new_sock->dev = local->dev;
-       new_sock->local = nfc_llcp_local_get(local);
-       new_sock->rw = sock->rw;
-       new_sock->miux = sock->miux;
-       new_sock->remote_miu = local->remote_miu;
-       new_sock->nfc_protocol = sock->nfc_protocol;
-       new_sock->dsap = ssap;
-       new_sock->target_idx = local->target_idx;
-       new_sock->parent = parent;
-       new_sock->ssap = sock->ssap;
-       if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
-               atomic_t *client_count;
-
-               pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
-
-               client_count =
-                       &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
-
-               atomic_inc(client_count);
-               new_sock->reserved_ssap = sock->ssap;
-       }
-
-       nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
-                                     skb->len - LLCP_HEADER_SIZE);
-
-       pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
-
-       nfc_llcp_sock_link(&local->sockets, new_sk);
-
-       nfc_llcp_accept_enqueue(&sock->sk, new_sk);
-
-       nfc_get_device(local->dev->idx);
-
-       new_sk->sk_state = LLCP_CONNECTED;
-
-       /* Wake the listening processes */
-       parent->sk_data_ready(parent, 0);
-
-       /* Send CC */
-       nfc_llcp_send_cc(new_sock);
-
-       release_sock(&sock->sk);
-       sock_put(&sock->sk);
-
-       return;
-
-fail:
-       /* Send DM */
-       nfc_llcp_send_dm(local, dsap, ssap, reason);
-}
-
-int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
-{
-       int nr_frames = 0;
-       struct nfc_llcp_local *local = sock->local;
-
-       pr_debug("Remote ready %d tx queue len %d remote rw %d",
-                sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
-                sock->remote_rw);
-
-       /* Try to queue some I frames for transmission */
-       while (sock->remote_ready &&
-              skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
-               struct sk_buff *pdu;
-
-               pdu = skb_dequeue(&sock->tx_queue);
-               if (pdu == NULL)
-                       break;
-
-               /* Update N(S)/N(R) */
-               nfc_llcp_set_nrns(sock, pdu);
-
-               skb_queue_tail(&local->tx_queue, pdu);
-               nr_frames++;
-       }
-
-       return nr_frames;
-}
-
-static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
-                              struct sk_buff *skb)
-{
-       struct nfc_llcp_sock *llcp_sock;
-       struct sock *sk;
-       u8 dsap, ssap, ptype, ns, nr;
-
-       ptype = nfc_llcp_ptype(skb);
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-       ns = nfc_llcp_ns(skb);
-       nr = nfc_llcp_nr(skb);
-
-       pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
-
-       llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
-       if (llcp_sock == NULL) {
-               nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
-               return;
-       }
-
-       sk = &llcp_sock->sk;
-       lock_sock(sk);
-       if (sk->sk_state == LLCP_CLOSED) {
-               release_sock(sk);
-               nfc_llcp_sock_put(llcp_sock);
-       }
-
-       /* Pass the payload upstream */
-       if (ptype == LLCP_PDU_I) {
-               pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
-
-               if (ns == llcp_sock->recv_n)
-                       llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
-               else
-                       pr_err("Received out of sequence I PDU\n");
-
-               skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
-               if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
-                       /*
-                        * I frames will be freed from the socket layer, so we
-                        * need to keep them alive until someone receives them.
-                        */
-                       skb_get(skb);
-               } else {
-                       pr_err("Receive queue is full\n");
-               }
-       }
-
-       /* Remove skbs from the pending queue */
-       if (llcp_sock->send_ack_n != nr) {
-               struct sk_buff *s, *tmp;
-               u8 n;
-
-               llcp_sock->send_ack_n = nr;
-
-               /* Remove and free all skbs until ns == nr */
-               skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
-                       n = nfc_llcp_ns(s);
-
-                       skb_unlink(s, &llcp_sock->tx_pending_queue);
-                       kfree_skb(s);
-
-                       if (n == nr)
-                               break;
-               }
-
-               /* Re-queue the remaining skbs for transmission */
-               skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
-                                           s, tmp) {
-                       skb_unlink(s, &llcp_sock->tx_pending_queue);
-                       skb_queue_head(&local->tx_queue, s);
-               }
-       }
-
-       if (ptype == LLCP_PDU_RR)
-               llcp_sock->remote_ready = true;
-       else if (ptype == LLCP_PDU_RNR)
-               llcp_sock->remote_ready = false;
-
-       if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
-               nfc_llcp_send_rr(llcp_sock);
-
-       release_sock(sk);
-       nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
-                              struct sk_buff *skb)
-{
-       struct nfc_llcp_sock *llcp_sock;
-       struct sock *sk;
-       u8 dsap, ssap;
-
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-
-       if ((dsap == 0) && (ssap == 0)) {
-               pr_debug("Connection termination");
-               nfc_dep_link_down(local->dev);
-               return;
-       }
-
-       llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
-       if (llcp_sock == NULL) {
-               nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
-               return;
-       }
-
-       sk = &llcp_sock->sk;
-       lock_sock(sk);
-
-       nfc_llcp_socket_purge(llcp_sock);
-
-       if (sk->sk_state == LLCP_CLOSED) {
-               release_sock(sk);
-               nfc_llcp_sock_put(llcp_sock);
-       }
-
-       if (sk->sk_state == LLCP_CONNECTED) {
-               nfc_put_device(local->dev);
-               sk->sk_state = LLCP_CLOSED;
-               sk->sk_state_change(sk);
-       }
-
-       nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
-
-       release_sock(sk);
-       nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
-       struct nfc_llcp_sock *llcp_sock;
-       struct sock *sk;
-       u8 dsap, ssap;
-
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-
-       llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
-       if (llcp_sock == NULL) {
-               pr_err("Invalid CC\n");
-               nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
-
-               return;
-       }
-
-       sk = &llcp_sock->sk;
-
-       /* Unlink from connecting and link to the client array */
-       nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
-       nfc_llcp_sock_link(&local->sockets, sk);
-       llcp_sock->dsap = ssap;
-
-       nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
-                                     skb->len - LLCP_HEADER_SIZE);
-
-       sk->sk_state = LLCP_CONNECTED;
-       sk->sk_state_change(sk);
-
-       nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
-       struct nfc_llcp_sock *llcp_sock;
-       struct sock *sk;
-       u8 dsap, ssap, reason;
-
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-       reason = skb->data[2];
-
-       pr_debug("%d %d reason %d\n", ssap, dsap, reason);
-
-       switch (reason) {
-       case LLCP_DM_NOBOUND:
-       case LLCP_DM_REJ:
-               llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
-               break;
-
-       default:
-               llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
-               break;
-       }
-
-       if (llcp_sock == NULL) {
-               pr_debug("Already closed\n");
-               return;
-       }
-
-       sk = &llcp_sock->sk;
-
-       sk->sk_err = ENXIO;
-       sk->sk_state = LLCP_CLOSED;
-       sk->sk_state_change(sk);
-
-       nfc_llcp_sock_put(llcp_sock);
-}
-
-static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
-                             struct sk_buff *skb)
-{
-       struct nfc_llcp_sock *llcp_sock;
-       u8 dsap, ssap, *tlv, type, length, tid, sap;
-       u16 tlv_len, offset;
-       char *service_name;
-       size_t service_name_len;
-       struct nfc_llcp_sdp_tlv *sdp;
-       HLIST_HEAD(llc_sdres_list);
-       size_t sdres_tlvs_len;
-       HLIST_HEAD(nl_sdres_list);
-
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-
-       pr_debug("%d %d\n", dsap, ssap);
-
-       if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
-               pr_err("Wrong SNL SAP\n");
-               return;
-       }
-
-       tlv = &skb->data[LLCP_HEADER_SIZE];
-       tlv_len = skb->len - LLCP_HEADER_SIZE;
-       offset = 0;
-       sdres_tlvs_len = 0;
-
-       while (offset < tlv_len) {
-               type = tlv[0];
-               length = tlv[1];
-
-               switch (type) {
-               case LLCP_TLV_SDREQ:
-                       tid = tlv[2];
-                       service_name = (char *) &tlv[3];
-                       service_name_len = length - 1;
-
-                       pr_debug("Looking for %.16s\n", service_name);
-
-                       if (service_name_len == strlen("urn:nfc:sn:sdp") &&
-                           !strncmp(service_name, "urn:nfc:sn:sdp",
-                                    service_name_len)) {
-                               sap = 1;
-                               goto add_snl;
-                       }
-
-                       llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
-                                                         service_name_len);
-                       if (!llcp_sock) {
-                               sap = 0;
-                               goto add_snl;
-                       }
-
-                       /*
-                        * We found a socket but its ssap has not been reserved
-                        * yet. We need to assign it for good and send a reply.
-                        * The ssap will be freed when the socket is closed.
-                        */
-                       if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
-                               atomic_t *client_count;
-
-                               sap = nfc_llcp_reserve_sdp_ssap(local);
-
-                               pr_debug("Reserving %d\n", sap);
-
-                               if (sap == LLCP_SAP_MAX) {
-                                       sap = 0;
-                                       goto add_snl;
-                               }
-
-                               client_count =
-                                       &local->local_sdp_cnt[sap -
-                                                             LLCP_WKS_NUM_SAP];
-
-                               atomic_inc(client_count);
-
-                               llcp_sock->ssap = sap;
-                               llcp_sock->reserved_ssap = sap;
-                       } else {
-                               sap = llcp_sock->ssap;
-                       }
-
-                       pr_debug("%p %d\n", llcp_sock, sap);
-
-add_snl:
-                       sdp = nfc_llcp_build_sdres_tlv(tid, sap);
-                       if (sdp == NULL)
-                               goto exit;
-
-                       sdres_tlvs_len += sdp->tlv_len;
-                       hlist_add_head(&sdp->node, &llc_sdres_list);
-                       break;
-
-               case LLCP_TLV_SDRES:
-                       mutex_lock(&local->sdreq_lock);
-
-                       pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
-
-                       hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
-                               if (sdp->tid != tlv[2])
-                                       continue;
-
-                               sdp->sap = tlv[3];
-
-                               pr_debug("Found: uri=%s, sap=%d\n",
-                                        sdp->uri, sdp->sap);
-
-                               hlist_del(&sdp->node);
-
-                               hlist_add_head(&sdp->node, &nl_sdres_list);
-
-                               break;
-                       }
-
-                       mutex_unlock(&local->sdreq_lock);
-                       break;
-
-               default:
-                       pr_err("Invalid SNL tlv value 0x%x\n", type);
-                       break;
-               }
-
-               offset += length + 2;
-               tlv += length + 2;
-       }
-
-exit:
-       if (!hlist_empty(&nl_sdres_list))
-               nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
-
-       if (!hlist_empty(&llc_sdres_list))
-               nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
-}
-
-static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
-       u8 ptype;
-       u16 pdu_len;
-       struct sk_buff *new_skb;
-
-       if (skb->len <= LLCP_HEADER_SIZE) {
-               pr_err("Malformed AGF PDU\n");
-               return;
-       }
-
-       skb_pull(skb, LLCP_HEADER_SIZE);
-
-       while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
-               pdu_len = skb->data[0] << 8 | skb->data[1];
-
-               skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
-
-               if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
-                       pr_err("Malformed AGF PDU\n");
-                       return;
-               }
-
-               ptype = nfc_llcp_ptype(skb);
-
-               if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
-                       goto next;
-
-               new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
-               if (new_skb == NULL) {
-                       pr_err("Could not allocate PDU\n");
-                       return;
-               }
-
-               memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
-
-               nfc_llcp_rx_skb(local, new_skb);
-
-               kfree_skb(new_skb);
-next:
-               skb_pull(skb, pdu_len);
-       }
-}
-
-static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
-       u8 dsap, ssap, ptype;
-
-       ptype = nfc_llcp_ptype(skb);
-       dsap = nfc_llcp_dsap(skb);
-       ssap = nfc_llcp_ssap(skb);
-
-       pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
-
-       if (ptype != LLCP_PDU_SYMM)
-               print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
-                              16, 1, skb->data, skb->len, true);
-
-       switch (ptype) {
-       case LLCP_PDU_SYMM:
-               pr_debug("SYMM\n");
-               break;
-
-       case LLCP_PDU_UI:
-               pr_debug("UI\n");
-               nfc_llcp_recv_ui(local, skb);
-               break;
-
-       case LLCP_PDU_CONNECT:
-               pr_debug("CONNECT\n");
-               nfc_llcp_recv_connect(local, skb);
-               break;
-
-       case LLCP_PDU_DISC:
-               pr_debug("DISC\n");
-               nfc_llcp_recv_disc(local, skb);
-               break;
-
-       case LLCP_PDU_CC:
-               pr_debug("CC\n");
-               nfc_llcp_recv_cc(local, skb);
-               break;
-
-       case LLCP_PDU_DM:
-               pr_debug("DM\n");
-               nfc_llcp_recv_dm(local, skb);
-               break;
-
-       case LLCP_PDU_SNL:
-               pr_debug("SNL\n");
-               nfc_llcp_recv_snl(local, skb);
-               break;
-
-       case LLCP_PDU_I:
-       case LLCP_PDU_RR:
-       case LLCP_PDU_RNR:
-               pr_debug("I frame\n");
-               nfc_llcp_recv_hdlc(local, skb);
-               break;
-
-       case LLCP_PDU_AGF:
-               pr_debug("AGF frame\n");
-               nfc_llcp_recv_agf(local, skb);
-               break;
-       }
-}
-
-static void nfc_llcp_rx_work(struct work_struct *work)
-{
-       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                   rx_work);
-       struct sk_buff *skb;
-
-       skb = local->rx_pending;
-       if (skb == NULL) {
-               pr_debug("No pending SKB\n");
-               return;
-       }
-
-       __net_timestamp(skb);
-
-       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
-
-       nfc_llcp_rx_skb(local, skb);
-
-       schedule_work(&local->tx_work);
-       kfree_skb(local->rx_pending);
-       local->rx_pending = NULL;
-}
-
-static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
-{
-       local->rx_pending = skb;
-       del_timer(&local->link_timer);
-       schedule_work(&local->rx_work);
-}
-
-void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
-{
-       struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
-
-       pr_debug("Received an LLCP PDU\n");
-       if (err < 0) {
-               pr_err("err %d\n", err);
-               return;
-       }
-
-       __nfc_llcp_recv(local, skb);
-}
-
-int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
-{
-       struct nfc_llcp_local *local;
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL)
-               return -ENODEV;
-
-       __nfc_llcp_recv(local, skb);
-
-       return 0;
-}
-
-void nfc_llcp_mac_is_down(struct nfc_dev *dev)
-{
-       struct nfc_llcp_local *local;
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL)
-               return;
-
-       local->remote_miu = LLCP_DEFAULT_MIU;
-       local->remote_lto = LLCP_DEFAULT_LTO;
-
-       /* Close and purge all existing sockets */
-       nfc_llcp_socket_release(local, true, 0);
-}
-
-void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
-                       u8 comm_mode, u8 rf_mode)
-{
-       struct nfc_llcp_local *local;
-
-       pr_debug("rf mode %d\n", rf_mode);
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL)
-               return;
-
-       local->target_idx = target_idx;
-       local->comm_mode = comm_mode;
-       local->rf_mode = rf_mode;
-
-       if (rf_mode == NFC_RF_INITIATOR) {
-               pr_debug("Queueing Tx work\n");
-
-               schedule_work(&local->tx_work);
-       } else {
-               mod_timer(&local->link_timer,
-                         jiffies + msecs_to_jiffies(local->remote_lto));
-       }
-}
-
-int nfc_llcp_register_device(struct nfc_dev *ndev)
-{
-       struct nfc_llcp_local *local;
-
-       local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
-       if (local == NULL)
-               return -ENOMEM;
-
-       local->dev = ndev;
-       INIT_LIST_HEAD(&local->list);
-       kref_init(&local->ref);
-       mutex_init(&local->sdp_lock);
-       init_timer(&local->link_timer);
-       local->link_timer.data = (unsigned long) local;
-       local->link_timer.function = nfc_llcp_symm_timer;
-
-       skb_queue_head_init(&local->tx_queue);
-       INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
-
-       local->rx_pending = NULL;
-       INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
-
-       INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
-
-       rwlock_init(&local->sockets.lock);
-       rwlock_init(&local->connecting_sockets.lock);
-       rwlock_init(&local->raw_sockets.lock);
-
-       local->lto = 150; /* 1500 ms */
-       local->rw = LLCP_MAX_RW;
-       local->miux = cpu_to_be16(LLCP_MAX_MIUX);
-
-       nfc_llcp_build_gb(local);
-
-       local->remote_miu = LLCP_DEFAULT_MIU;
-       local->remote_lto = LLCP_DEFAULT_LTO;
-
-       mutex_init(&local->sdreq_lock);
-       INIT_HLIST_HEAD(&local->pending_sdreqs);
-       init_timer(&local->sdreq_timer);
-       local->sdreq_timer.data = (unsigned long) local;
-       local->sdreq_timer.function = nfc_llcp_sdreq_timer;
-       INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
-
-       list_add(&local->list, &llcp_devices);
-
-       return 0;
-}
-
-void nfc_llcp_unregister_device(struct nfc_dev *dev)
-{
-       struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
-
-       if (local == NULL) {
-               pr_debug("No such device\n");
-               return;
-       }
-
-       local_cleanup(local);
-
-       nfc_llcp_local_put(local);
-}
-
-int __init nfc_llcp_init(void)
-{
-       INIT_LIST_HEAD(&llcp_devices);
-
-       return nfc_llcp_sock_init();
-}
-
-void nfc_llcp_exit(void)
-{
-       nfc_llcp_sock_exit();
-}
diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
deleted file mode 100644 (file)
index ff8c434..0000000
+++ /dev/null
@@ -1,267 +0,0 @@
-/*
- * Copyright (C) 2011  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 as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the
- * Free Software Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-enum llcp_state {
-       LLCP_CONNECTED = 1, /* wait_for_packet() wants that */
-       LLCP_CLOSED,
-       LLCP_BOUND,
-       LLCP_LISTEN,
-};
-
-#define LLCP_DEFAULT_LTO 100
-#define LLCP_DEFAULT_RW  1
-#define LLCP_DEFAULT_MIU 128
-
-#define LLCP_MAX_LTO  0xff
-#define LLCP_MAX_RW   15
-#define LLCP_MAX_MIUX 0x7ff
-#define LLCP_MAX_MIU (LLCP_MAX_MIUX + 128)
-
-#define LLCP_WKS_NUM_SAP   16
-#define LLCP_SDP_NUM_SAP   16
-#define LLCP_LOCAL_NUM_SAP 32
-#define LLCP_LOCAL_SAP_OFFSET (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP)
-#define LLCP_MAX_SAP (LLCP_WKS_NUM_SAP + LLCP_SDP_NUM_SAP + LLCP_LOCAL_NUM_SAP)
-#define LLCP_SDP_UNBOUND   (LLCP_MAX_SAP + 1)
-
-struct nfc_llcp_sock;
-
-struct llcp_sock_list {
-       struct hlist_head head;
-       rwlock_t          lock;
-};
-
-struct nfc_llcp_sdp_tlv {
-       u8 *tlv;
-       u8 tlv_len;
-
-       char *uri;
-       u8 tid;
-       u8 sap;
-
-       unsigned long time;
-
-       struct hlist_node node;
-};
-
-struct nfc_llcp_local {
-       struct list_head list;
-       struct nfc_dev *dev;
-
-       struct kref ref;
-
-       struct mutex sdp_lock;
-
-       struct timer_list link_timer;
-       struct sk_buff_head tx_queue;
-       struct work_struct       tx_work;
-       struct work_struct       rx_work;
-       struct sk_buff *rx_pending;
-       struct work_struct       timeout_work;
-
-       u32 target_idx;
-       u8 rf_mode;
-       u8 comm_mode;
-       u8 lto;
-       u8 rw;
-       __be16 miux;
-       unsigned long local_wks;      /* Well known services */
-       unsigned long local_sdp;      /* Local services  */
-       unsigned long local_sap; /* Local SAPs, not available for discovery */
-       atomic_t local_sdp_cnt[LLCP_SDP_NUM_SAP];
-
-       /* local */
-       u8 gb[NFC_MAX_GT_LEN];
-       u8 gb_len;
-
-       /* remote */
-       u8 remote_gb[NFC_MAX_GT_LEN];
-       u8 remote_gb_len;
-
-       u8  remote_version;
-       u16 remote_miu;
-       u16 remote_lto;
-       u8  remote_opt;
-       u16 remote_wks;
-
-       struct mutex sdreq_lock;
-       struct hlist_head pending_sdreqs;
-       struct timer_list sdreq_timer;
-       struct work_struct sdreq_timeout_work;
-       u8 sdreq_next_tid;
-
-       /* sockets array */
-       struct llcp_sock_list sockets;
-       struct llcp_sock_list connecting_sockets;
-       struct llcp_sock_list raw_sockets;
-};
-
-struct nfc_llcp_sock {
-       struct sock sk;
-       struct nfc_dev *dev;
-       struct nfc_llcp_local *local;
-       u32 target_idx;
-       u32 nfc_protocol;
-
-       /* Link parameters */
-       u8 ssap;
-       u8 dsap;
-       char *service_name;
-       size_t service_name_len;
-       u8 rw;
-       __be16 miux;
-
-
-       /* Remote link parameters */
-       u8 remote_rw;
-       u16 remote_miu;
-
-       /* Link variables */
-       u8 send_n;
-       u8 send_ack_n;
-       u8 recv_n;
-       u8 recv_ack_n;
-
-       /* Is the remote peer ready to receive */
-       u8 remote_ready;
-
-       /* Reserved source SAP */
-       u8 reserved_ssap;
-
-       struct sk_buff_head tx_queue;
-       struct sk_buff_head tx_pending_queue;
-
-       struct list_head accept_queue;
-       struct sock *parent;
-};
-
-struct nfc_llcp_ui_cb {
-       __u8 dsap;
-       __u8 ssap;
-};
-
-#define nfc_llcp_ui_skb_cb(__skb) ((struct nfc_llcp_ui_cb *)&((__skb)->cb[0]))
-
-#define nfc_llcp_sock(sk) ((struct nfc_llcp_sock *) (sk))
-#define nfc_llcp_dev(sk)  (nfc_llcp_sock((sk))->dev)
-
-#define LLCP_HEADER_SIZE   2
-#define LLCP_SEQUENCE_SIZE 1
-#define LLCP_AGF_PDU_HEADER_SIZE 2
-
-/* LLCP versions: 1.1 is 1.0 plus SDP */
-#define LLCP_VERSION_10 0x10
-#define LLCP_VERSION_11 0x11
-
-/* LLCP PDU types */
-#define LLCP_PDU_SYMM     0x0
-#define LLCP_PDU_PAX      0x1
-#define LLCP_PDU_AGF      0x2
-#define LLCP_PDU_UI       0x3
-#define LLCP_PDU_CONNECT  0x4
-#define LLCP_PDU_DISC     0x5
-#define LLCP_PDU_CC       0x6
-#define LLCP_PDU_DM       0x7
-#define LLCP_PDU_FRMR     0x8
-#define LLCP_PDU_SNL      0x9
-#define LLCP_PDU_I        0xc
-#define LLCP_PDU_RR       0xd
-#define LLCP_PDU_RNR      0xe
-
-/* Parameters TLV types */
-#define LLCP_TLV_VERSION 0x1
-#define LLCP_TLV_MIUX    0x2
-#define LLCP_TLV_WKS     0x3
-#define LLCP_TLV_LTO     0x4
-#define LLCP_TLV_RW      0x5
-#define LLCP_TLV_SN      0x6
-#define LLCP_TLV_OPT     0x7
-#define LLCP_TLV_SDREQ   0x8
-#define LLCP_TLV_SDRES   0x9
-#define LLCP_TLV_MAX     0xa
-
-/* Well known LLCP SAP */
-#define LLCP_SAP_SDP   0x1
-#define LLCP_SAP_IP    0x2
-#define LLCP_SAP_OBEX  0x3
-#define LLCP_SAP_SNEP  0x4
-#define LLCP_SAP_MAX   0xff
-
-/* Disconnection reason code */
-#define LLCP_DM_DISC    0x00
-#define LLCP_DM_NOCONN  0x01
-#define LLCP_DM_NOBOUND 0x02
-#define LLCP_DM_REJ     0x03
-
-
-void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *s);
-void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *s);
-void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock);
-struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev);
-struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local);
-int nfc_llcp_local_put(struct nfc_llcp_local *local);
-u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
-                        struct nfc_llcp_sock *sock);
-u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local);
-void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap);
-int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock);
-void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
-                              struct sk_buff *skb, u8 direction);
-
-/* Sock API */
-struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp);
-void nfc_llcp_sock_free(struct nfc_llcp_sock *sock);
-void nfc_llcp_accept_unlink(struct sock *sk);
-void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk);
-struct sock *nfc_llcp_accept_dequeue(struct sock *sk, struct socket *newsock);
-
-/* TLV API */
-int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
-                         u8 *tlv_array, u16 tlv_array_len);
-int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
-                                 u8 *tlv_array, u16 tlv_array_len);
-
-/* Commands API */
-void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
-u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length);
-struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap);
-struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
-                                                 size_t uri_len);
-void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
-void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head);
-void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);
-int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);
-int nfc_llcp_send_symm(struct nfc_dev *dev);
-int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);
-int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);
-int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
-                           struct hlist_head *tlv_list, size_t tlvs_len);
-int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
-                           struct hlist_head *tlv_list, size_t tlvs_len);
-int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);
-int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);
-int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
-                         struct msghdr *msg, size_t len);
-int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
-                          struct msghdr *msg, size_t len);
-int nfc_llcp_send_rr(struct nfc_llcp_sock *sock);
-
-/* Socket API */
-int __init nfc_llcp_sock_init(void);
-void nfc_llcp_sock_exit(void);
diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c
deleted file mode 100644 (file)
index d6faa47..0000000
+++ /dev/null
@@ -1,1029 +0,0 @@
-/*
- * Copyright (C) 2011  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 as published by
- * the Free Software Foundation; either version 2 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program; if not, write to the
- * Free Software Foundation, Inc.,
- * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
- */
-
-#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
-
-#include <linux/init.h>
-#include <linux/kernel.h>
-#include <linux/module.h>
-#include <linux/nfc.h>
-
-#include "../nfc.h"
-#include "llcp.h"
-
-static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
-{
-       DECLARE_WAITQUEUE(wait, current);
-       int err = 0;
-
-       pr_debug("sk %p", sk);
-
-       add_wait_queue(sk_sleep(sk), &wait);
-       set_current_state(TASK_INTERRUPTIBLE);
-
-       while (sk->sk_state != state) {
-               if (!timeo) {
-                       err = -EINPROGRESS;
-                       break;
-               }
-
-               if (signal_pending(current)) {
-                       err = sock_intr_errno(timeo);
-                       break;
-               }
-
-               release_sock(sk);
-               timeo = schedule_timeout(timeo);
-               lock_sock(sk);
-               set_current_state(TASK_INTERRUPTIBLE);
-
-               err = sock_error(sk);
-               if (err)
-                       break;
-       }
-
-       __set_current_state(TASK_RUNNING);
-       remove_wait_queue(sk_sleep(sk), &wait);
-       return err;
-}
-
-static struct proto llcp_sock_proto = {
-       .name     = "NFC_LLCP",
-       .owner    = THIS_MODULE,
-       .obj_size = sizeof(struct nfc_llcp_sock),
-};
-
-static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       struct nfc_llcp_local *local;
-       struct nfc_dev *dev;
-       struct sockaddr_nfc_llcp llcp_addr;
-       int len, ret = 0;
-
-       if (!addr || addr->sa_family != AF_NFC)
-               return -EINVAL;
-
-       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
-
-       memset(&llcp_addr, 0, sizeof(llcp_addr));
-       len = min_t(unsigned int, sizeof(llcp_addr), alen);
-       memcpy(&llcp_addr, addr, len);
-
-       /* This is going to be a listening socket, dsap must be 0 */
-       if (llcp_addr.dsap != 0)
-               return -EINVAL;
-
-       lock_sock(sk);
-
-       if (sk->sk_state != LLCP_CLOSED) {
-               ret = -EBADFD;
-               goto error;
-       }
-
-       dev = nfc_get_device(llcp_addr.dev_idx);
-       if (dev == NULL) {
-               ret = -ENODEV;
-               goto error;
-       }
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL) {
-               ret = -ENODEV;
-               goto put_dev;
-       }
-
-       llcp_sock->dev = dev;
-       llcp_sock->local = nfc_llcp_local_get(local);
-       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
-       llcp_sock->service_name_len = min_t(unsigned int,
-                                           llcp_addr.service_name_len,
-                                           NFC_LLCP_MAX_SERVICE_NAME);
-       llcp_sock->service_name = kmemdup(llcp_addr.service_name,
-                                         llcp_sock->service_name_len,
-                                         GFP_KERNEL);
-
-       llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
-       if (llcp_sock->ssap == LLCP_SAP_MAX) {
-               ret = -EADDRINUSE;
-               goto put_dev;
-       }
-
-       llcp_sock->reserved_ssap = llcp_sock->ssap;
-
-       nfc_llcp_sock_link(&local->sockets, sk);
-
-       pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
-
-       sk->sk_state = LLCP_BOUND;
-
-put_dev:
-       nfc_put_device(dev);
-
-error:
-       release_sock(sk);
-       return ret;
-}
-
-static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
-                             int alen)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       struct nfc_llcp_local *local;
-       struct nfc_dev *dev;
-       struct sockaddr_nfc_llcp llcp_addr;
-       int len, ret = 0;
-
-       if (!addr || addr->sa_family != AF_NFC)
-               return -EINVAL;
-
-       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
-
-       memset(&llcp_addr, 0, sizeof(llcp_addr));
-       len = min_t(unsigned int, sizeof(llcp_addr), alen);
-       memcpy(&llcp_addr, addr, len);
-
-       lock_sock(sk);
-
-       if (sk->sk_state != LLCP_CLOSED) {
-               ret = -EBADFD;
-               goto error;
-       }
-
-       dev = nfc_get_device(llcp_addr.dev_idx);
-       if (dev == NULL) {
-               ret = -ENODEV;
-               goto error;
-       }
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL) {
-               ret = -ENODEV;
-               goto put_dev;
-       }
-
-       llcp_sock->dev = dev;
-       llcp_sock->local = nfc_llcp_local_get(local);
-       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
-
-       nfc_llcp_sock_link(&local->raw_sockets, sk);
-
-       sk->sk_state = LLCP_BOUND;
-
-put_dev:
-       nfc_put_device(dev);
-
-error:
-       release_sock(sk);
-       return ret;
-}
-
-static int llcp_sock_listen(struct socket *sock, int backlog)
-{
-       struct sock *sk = sock->sk;
-       int ret = 0;
-
-       pr_debug("sk %p backlog %d\n", sk, backlog);
-
-       lock_sock(sk);
-
-       if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
-           sk->sk_state != LLCP_BOUND) {
-               ret = -EBADFD;
-               goto error;
-       }
-
-       sk->sk_max_ack_backlog = backlog;
-       sk->sk_ack_backlog = 0;
-
-       pr_debug("Socket listening\n");
-       sk->sk_state = LLCP_LISTEN;
-
-error:
-       release_sock(sk);
-
-       return ret;
-}
-
-static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
-                              char __user *optval, unsigned int optlen)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       u32 opt;
-       int err = 0;
-
-       pr_debug("%p optname %d\n", sk, optname);
-
-       if (level != SOL_NFC)
-               return -ENOPROTOOPT;
-
-       lock_sock(sk);
-
-       switch (optname) {
-       case NFC_LLCP_RW:
-               if (sk->sk_state == LLCP_CONNECTED ||
-                   sk->sk_state == LLCP_BOUND ||
-                   sk->sk_state == LLCP_LISTEN) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               if (get_user(opt, (u32 __user *) optval)) {
-                       err = -EFAULT;
-                       break;
-               }
-
-               if (opt > LLCP_MAX_RW) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               llcp_sock->rw = (u8) opt;
-
-               break;
-
-       case NFC_LLCP_MIUX:
-               if (sk->sk_state == LLCP_CONNECTED ||
-                   sk->sk_state == LLCP_BOUND ||
-                   sk->sk_state == LLCP_LISTEN) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               if (get_user(opt, (u32 __user *) optval)) {
-                       err = -EFAULT;
-                       break;
-               }
-
-               if (opt > LLCP_MAX_MIUX) {
-                       err = -EINVAL;
-                       break;
-               }
-
-               llcp_sock->miux = cpu_to_be16((u16) opt);
-
-               break;
-
-       default:
-               err = -ENOPROTOOPT;
-               break;
-       }
-
-       release_sock(sk);
-
-       pr_debug("%p rw %d miux %d\n", llcp_sock,
-                llcp_sock->rw, llcp_sock->miux);
-
-       return err;
-}
-
-static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
-                              char __user *optval, int __user *optlen)
-{
-       struct nfc_llcp_local *local;
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       int len, err = 0;
-       u16 miux, remote_miu;
-       u8 rw;
-
-       pr_debug("%p optname %d\n", sk, optname);
-
-       if (level != SOL_NFC)
-               return -ENOPROTOOPT;
-
-       if (get_user(len, optlen))
-               return -EFAULT;
-
-       local = llcp_sock->local;
-       if (!local)
-               return -ENODEV;
-
-       len = min_t(u32, len, sizeof(u32));
-
-       lock_sock(sk);
-
-       switch (optname) {
-       case NFC_LLCP_RW:
-               rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
-               if (put_user(rw, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_MIUX:
-               miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
-                       be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
-
-               if (put_user(miux, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_REMOTE_MIU:
-               remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
-                               local->remote_miu : llcp_sock->remote_miu;
-
-               if (put_user(remote_miu, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_REMOTE_LTO:
-               if (put_user(local->remote_lto / 10, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       case NFC_LLCP_REMOTE_RW:
-               if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
-                       err = -EFAULT;
-
-               break;
-
-       default:
-               err = -ENOPROTOOPT;
-               break;
-       }
-
-       release_sock(sk);
-
-       if (put_user(len, optlen))
-               return -EFAULT;
-
-       return err;
-}
-
-void nfc_llcp_accept_unlink(struct sock *sk)
-{
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-
-       pr_debug("state %d\n", sk->sk_state);
-
-       list_del_init(&llcp_sock->accept_queue);
-       sk_acceptq_removed(llcp_sock->parent);
-       llcp_sock->parent = NULL;
-
-       sock_put(sk);
-}
-
-void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
-{
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
-
-       /* Lock will be free from unlink */
-       sock_hold(sk);
-
-       list_add_tail(&llcp_sock->accept_queue,
-                     &llcp_sock_parent->accept_queue);
-       llcp_sock->parent = parent;
-       sk_acceptq_added(parent);
-}
-
-struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
-                                    struct socket *newsock)
-{
-       struct nfc_llcp_sock *lsk, *n, *llcp_parent;
-       struct sock *sk;
-
-       llcp_parent = nfc_llcp_sock(parent);
-
-       list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
-                                accept_queue) {
-               sk = &lsk->sk;
-               lock_sock(sk);
-
-               if (sk->sk_state == LLCP_CLOSED) {
-                       release_sock(sk);
-                       nfc_llcp_accept_unlink(sk);
-                       continue;
-               }
-
-               if (sk->sk_state == LLCP_CONNECTED || !newsock) {
-                       list_del_init(&lsk->accept_queue);
-                       sock_put(sk);
-
-                       if (newsock)
-                               sock_graft(sk, newsock);
-
-                       release_sock(sk);
-
-                       pr_debug("Returning sk state %d\n", sk->sk_state);
-
-                       sk_acceptq_removed(parent);
-
-                       return sk;
-               }
-
-               release_sock(sk);
-       }
-
-       return NULL;
-}
-
-static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
-                           int flags)
-{
-       DECLARE_WAITQUEUE(wait, current);
-       struct sock *sk = sock->sk, *new_sk;
-       long timeo;
-       int ret = 0;
-
-       pr_debug("parent %p\n", sk);
-
-       lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
-
-       if (sk->sk_state != LLCP_LISTEN) {
-               ret = -EBADFD;
-               goto error;
-       }
-
-       timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
-
-       /* Wait for an incoming connection. */
-       add_wait_queue_exclusive(sk_sleep(sk), &wait);
-       while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
-               set_current_state(TASK_INTERRUPTIBLE);
-
-               if (!timeo) {
-                       ret = -EAGAIN;
-                       break;
-               }
-
-               if (signal_pending(current)) {
-                       ret = sock_intr_errno(timeo);
-                       break;
-               }
-
-               release_sock(sk);
-               timeo = schedule_timeout(timeo);
-               lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
-       }
-       __set_current_state(TASK_RUNNING);
-       remove_wait_queue(sk_sleep(sk), &wait);
-
-       if (ret)
-               goto error;
-
-       newsock->state = SS_CONNECTED;
-
-       pr_debug("new socket %p\n", new_sk);
-
-error:
-       release_sock(sk);
-
-       return ret;
-}
-
-static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr,
-                            int *len, int peer)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr);
-
-       if (llcp_sock == NULL || llcp_sock->dev == NULL)
-               return -EBADFD;
-
-       pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx,
-                llcp_sock->dsap, llcp_sock->ssap);
-
-       uaddr->sa_family = AF_NFC;
-
-       *len = sizeof(struct sockaddr_nfc_llcp);
-
-       llcp_addr->dev_idx = llcp_sock->dev->idx;
-       llcp_addr->target_idx = llcp_sock->target_idx;
-       llcp_addr->dsap = llcp_sock->dsap;
-       llcp_addr->ssap = llcp_sock->ssap;
-       llcp_addr->service_name_len = llcp_sock->service_name_len;
-       memcpy(llcp_addr->service_name, llcp_sock->service_name,
-              llcp_addr->service_name_len);
-
-       return 0;
-}
-
-static inline unsigned int llcp_accept_poll(struct sock *parent)
-{
-       struct nfc_llcp_sock *llcp_sock, *n, *parent_sock;
-       struct sock *sk;
-
-       parent_sock = nfc_llcp_sock(parent);
-
-       list_for_each_entry_safe(llcp_sock, n, &parent_sock->accept_queue,
-                                accept_queue) {
-               sk = &llcp_sock->sk;
-
-               if (sk->sk_state == LLCP_CONNECTED)
-                       return POLLIN | POLLRDNORM;
-       }
-
-       return 0;
-}
-
-static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
-                                  poll_table *wait)
-{
-       struct sock *sk = sock->sk;
-       unsigned int mask = 0;
-
-       pr_debug("%p\n", sk);
-
-       sock_poll_wait(file, sk_sleep(sk), wait);
-
-       if (sk->sk_state == LLCP_LISTEN)
-               return llcp_accept_poll(sk);
-
-       if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
-               mask |= POLLERR |
-                       (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? POLLPRI : 0);
-
-       if (!skb_queue_empty(&sk->sk_receive_queue))
-               mask |= POLLIN | POLLRDNORM;
-
-       if (sk->sk_state == LLCP_CLOSED)
-               mask |= POLLHUP;
-
-       if (sk->sk_shutdown & RCV_SHUTDOWN)
-               mask |= POLLRDHUP | POLLIN | POLLRDNORM;
-
-       if (sk->sk_shutdown == SHUTDOWN_MASK)
-               mask |= POLLHUP;
-
-       if (sock_writeable(sk))
-               mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
-       else
-               set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags);
-
-       pr_debug("mask 0x%x\n", mask);
-
-       return mask;
-}
-
-static int llcp_sock_release(struct socket *sock)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_local *local;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       int err = 0;
-
-       if (!sk)
-               return 0;
-
-       pr_debug("%p\n", sk);
-
-       local = llcp_sock->local;
-       if (local == NULL) {
-               err = -ENODEV;
-               goto out;
-       }
-
-       lock_sock(sk);
-
-       /* Send a DISC */
-       if (sk->sk_state == LLCP_CONNECTED)
-               nfc_llcp_disconnect(llcp_sock);
-
-       if (sk->sk_state == LLCP_LISTEN) {
-               struct nfc_llcp_sock *lsk, *n;
-               struct sock *accept_sk;
-
-               list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
-                                        accept_queue) {
-                       accept_sk = &lsk->sk;
-                       lock_sock(accept_sk);
-
-                       nfc_llcp_disconnect(lsk);
-                       nfc_llcp_accept_unlink(accept_sk);
-
-                       release_sock(accept_sk);
-               }
-       }
-
-       if (llcp_sock->reserved_ssap < LLCP_SAP_MAX)
-               nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
-
-       release_sock(sk);
-
-       if (sock->type == SOCK_RAW)
-               nfc_llcp_sock_unlink(&local->raw_sockets, sk);
-       else
-               nfc_llcp_sock_unlink(&local->sockets, sk);
-
-out:
-       sock_orphan(sk);
-       sock_put(sk);
-
-       return err;
-}
-
-static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
-                            int len, int flags)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
-       struct nfc_dev *dev;
-       struct nfc_llcp_local *local;
-       int ret = 0;
-
-       pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
-
-       if (!addr || len < sizeof(struct sockaddr_nfc) ||
-           addr->sa_family != AF_NFC)
-               return -EINVAL;
-
-       if (addr->service_name_len == 0 && addr->dsap == 0)
-               return -EINVAL;
-
-       pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
-                addr->target_idx, addr->nfc_protocol);
-
-       lock_sock(sk);
-
-       if (sk->sk_state == LLCP_CONNECTED) {
-               ret = -EISCONN;
-               goto error;
-       }
-
-       dev = nfc_get_device(addr->dev_idx);
-       if (dev == NULL) {
-               ret = -ENODEV;
-               goto error;
-       }
-
-       local = nfc_llcp_find_local(dev);
-       if (local == NULL) {
-               ret = -ENODEV;
-               goto put_dev;
-       }
-
-       device_lock(&dev->dev);
-       if (dev->dep_link_up == false) {
-               ret = -ENOLINK;
-               device_unlock(&dev->dev);
-               goto put_dev;
-       }
-       device_unlock(&dev->dev);
-
-       if (local->rf_mode == NFC_RF_INITIATOR &&
-           addr->target_idx != local->target_idx) {
-               ret = -ENOLINK;
-               goto put_dev;
-       }
-
-       llcp_sock->dev = dev;
-       llcp_sock->local = nfc_llcp_local_get(local);
-       llcp_sock->remote_miu = llcp_sock->local->remote_miu;
-       llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
-       if (llcp_sock->ssap == LLCP_SAP_MAX) {
-               ret = -ENOMEM;
-               goto put_dev;
-       }
-
-       llcp_sock->reserved_ssap = llcp_sock->ssap;
-
-       if (addr->service_name_len == 0)
-               llcp_sock->dsap = addr->dsap;
-       else
-               llcp_sock->dsap = LLCP_SAP_SDP;
-       llcp_sock->nfc_protocol = addr->nfc_protocol;
-       llcp_sock->service_name_len = min_t(unsigned int,
-                                           addr->service_name_len,
-                                           NFC_LLCP_MAX_SERVICE_NAME);
-       llcp_sock->service_name = kmemdup(addr->service_name,
-                                         llcp_sock->service_name_len,
-                                         GFP_KERNEL);
-
-       nfc_llcp_sock_link(&local->connecting_sockets, sk);
-
-       ret = nfc_llcp_send_connect(llcp_sock);
-       if (ret)
-               goto sock_unlink;
-
-       ret = sock_wait_state(sk, LLCP_CONNECTED,
-                             sock_sndtimeo(sk, flags & O_NONBLOCK));
-       if (ret)
-               goto sock_unlink;
-
-       release_sock(sk);
-
-       return 0;
-
-sock_unlink:
-       nfc_llcp_put_ssap(local, llcp_sock->ssap);
-
-       nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
-
-put_dev:
-       nfc_put_device(dev);
-
-error:
-       release_sock(sk);
-       return ret;
-}
-
-static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
-                            struct msghdr *msg, size_t len)
-{
-       struct sock *sk = sock->sk;
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-       int ret;
-
-       pr_debug("sock %p sk %p", sock, sk);
-
-       ret = sock_error(sk);
-       if (ret)
-               return ret;
-
-       if (msg->msg_flags & MSG_OOB)
-               return -EOPNOTSUPP;
-
-       lock_sock(sk);
-
-       if (sk->sk_type == SOCK_DGRAM) {
-               struct sockaddr_nfc_llcp *addr =
-                       (struct sockaddr_nfc_llcp *)msg->msg_name;
-
-               if (msg->msg_namelen < sizeof(*addr)) {
-                       release_sock(sk);
-                       return -EINVAL;
-               }
-
-               release_sock(sk);
-
-               return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
-                                             msg, len);
-       }
-
-       if (sk->sk_state != LLCP_CONNECTED) {
-               release_sock(sk);
-               return -ENOTCONN;
-       }
-
-       release_sock(sk);
-
-       return nfc_llcp_send_i_frame(llcp_sock, msg, len);
-}
-
-static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
-                            struct msghdr *msg, size_t len, int flags)
-{
-       int noblock = flags & MSG_DONTWAIT;
-       struct sock *sk = sock->sk;
-       unsigned int copied, rlen;
-       struct sk_buff *skb, *cskb;
-       int err = 0;
-
-       pr_debug("%p %zu\n", sk, len);
-
-       msg->msg_namelen = 0;
-
-       lock_sock(sk);
-
-       if (sk->sk_state == LLCP_CLOSED &&
-           skb_queue_empty(&sk->sk_receive_queue)) {
-               release_sock(sk);
-               return 0;
-       }
-
-       release_sock(sk);
-
-       if (flags & (MSG_OOB))
-               return -EOPNOTSUPP;
-
-       skb = skb_recv_datagram(sk, flags, noblock, &err);
-       if (!skb) {
-               pr_err("Recv datagram failed state %d %d %d",
-                      sk->sk_state, err, sock_error(sk));
-
-               if (sk->sk_shutdown & RCV_SHUTDOWN)
-                       return 0;
-
-               return err;
-       }
-
-       rlen = skb->len;                /* real length of skb */
-       copied = min_t(unsigned int, rlen, len);
-
-       cskb = skb;
-       if (skb_copy_datagram_iovec(cskb, 0, msg->msg_iov, copied)) {
-               if (!(flags & MSG_PEEK))
-                       skb_queue_head(&sk->sk_receive_queue, skb);
-               return -EFAULT;
-       }
-
-       sock_recv_timestamp(msg, sk, skb);
-
-       if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
-               struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
-               struct sockaddr_nfc_llcp *sockaddr =
-                       (struct sockaddr_nfc_llcp *) msg->msg_name;
-
-               msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp);
-
-               pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
-
-               memset(sockaddr, 0, sizeof(*sockaddr));
-               sockaddr->sa_family = AF_NFC;
-               sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
-               sockaddr->dsap = ui_cb->dsap;
-               sockaddr->ssap = ui_cb->ssap;
-       }
-
-       /* Mark read part of skb as used */
-       if (!(flags & MSG_PEEK)) {
-
-               /* SOCK_STREAM: re-queue skb if it contains unreceived data */
-               if (sk->sk_type == SOCK_STREAM ||
-                   sk->sk_type == SOCK_DGRAM ||
-                   sk->sk_type == SOCK_RAW) {
-                       skb_pull(skb, copied);
-                       if (skb->len) {
-                               skb_queue_head(&sk->sk_receive_queue, skb);
-                               goto done;
-                       }
-               }
-
-               kfree_skb(skb);
-       }
-
-       /* XXX Queue backlogged skbs */
-
-done:
-       /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
-       if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
-               copied = rlen;
-
-       return copied;
-}
-
-static const struct proto_ops llcp_sock_ops = {
-       .family         = PF_NFC,
-       .owner          = THIS_MODULE,
-       .bind           = llcp_sock_bind,
-       .connect        = llcp_sock_connect,
-       .release        = llcp_sock_release,
-       .socketpair     = sock_no_socketpair,
-       .accept         = llcp_sock_accept,
-       .getname        = llcp_sock_getname,
-       .poll           = llcp_sock_poll,
-       .ioctl          = sock_no_ioctl,
-       .listen         = llcp_sock_listen,
-       .shutdown       = sock_no_shutdown,
-       .setsockopt     = nfc_llcp_setsockopt,
-       .getsockopt     = nfc_llcp_getsockopt,
-       .sendmsg        = llcp_sock_sendmsg,
-       .recvmsg        = llcp_sock_recvmsg,
-       .mmap           = sock_no_mmap,
-};
-
-static const struct proto_ops llcp_rawsock_ops = {
-       .family         = PF_NFC,
-       .owner          = THIS_MODULE,
-       .bind           = llcp_raw_sock_bind,
-       .connect        = sock_no_connect,
-       .release        = llcp_sock_release,
-       .socketpair     = sock_no_socketpair,
-       .accept         = sock_no_accept,
-       .getname        = llcp_sock_getname,
-       .poll           = llcp_sock_poll,
-       .ioctl          = sock_no_ioctl,
-       .listen         = sock_no_listen,
-       .shutdown       = sock_no_shutdown,
-       .setsockopt     = sock_no_setsockopt,
-       .getsockopt     = sock_no_getsockopt,
-       .sendmsg        = sock_no_sendmsg,
-       .recvmsg        = llcp_sock_recvmsg,
-       .mmap           = sock_no_mmap,
-};
-
-static void llcp_sock_destruct(struct sock *sk)
-{
-       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
-
-       pr_debug("%p\n", sk);
-
-       if (sk->sk_state == LLCP_CONNECTED)
-               nfc_put_device(llcp_sock->dev);
-
-       skb_queue_purge(&sk->sk_receive_queue);
-
-       nfc_llcp_sock_free(llcp_sock);
-
-       if (!sock_flag(sk, SOCK_DEAD)) {
-               pr_err("Freeing alive NFC LLCP socket %p\n", sk);
-               return;
-       }
-}
-
-struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
-{
-       struct sock *sk;
-       struct nfc_llcp_sock *llcp_sock;
-
-       sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto);
-       if (!sk)
-               return NULL;
-
-       llcp_sock = nfc_llcp_sock(sk);
-
-       sock_init_data(sock, sk);
-       sk->sk_state = LLCP_CLOSED;
-       sk->sk_protocol = NFC_SOCKPROTO_LLCP;
-       sk->sk_type = type;
-       sk->sk_destruct = llcp_sock_destruct;
-
-       llcp_sock->ssap = 0;
-       llcp_sock->dsap = LLCP_SAP_SDP;
-       llcp_sock->rw = LLCP_MAX_RW + 1;
-       llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1);
-       llcp_sock->send_n = llcp_sock->send_ack_n = 0;
-       llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
-       llcp_sock->remote_ready = 1;
-       llcp_sock->reserved_ssap = LLCP_SAP_MAX;
-       nfc_llcp_socket_remote_param_init(llcp_sock);
-       skb_queue_head_init(&llcp_sock->tx_queue);
-       skb_queue_head_init(&llcp_sock->tx_pending_queue);
-       INIT_LIST_HEAD(&llcp_sock->accept_queue);
-
-       if (sock != NULL)
-               sock->state = SS_UNCONNECTED;
-
-       return sk;
-}
-
-void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
-{
-       kfree(sock->service_name);
-
-       skb_queue_purge(&sock->tx_queue);
-       skb_queue_purge(&sock->tx_pending_queue);
-
-       list_del_init(&sock->accept_queue);
-
-       sock->parent = NULL;
-
-       nfc_llcp_local_put(sock->local);
-}
-
-static int llcp_sock_create(struct net *net, struct socket *sock,
-                           const struct nfc_protocol *nfc_proto)
-{
-       struct sock *sk;
-
-       pr_debug("%p\n", sock);
-
-       if (sock->type != SOCK_STREAM &&
-           sock->type != SOCK_DGRAM &&
-           sock->type != SOCK_RAW)
-               return -ESOCKTNOSUPPORT;
-
-       if (sock->type == SOCK_RAW)
-               sock->ops = &llcp_rawsock_ops;
-       else
-               sock->ops = &llcp_sock_ops;
-
-       sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
-       if (sk == NULL)
-               return -ENOMEM;
-
-       return 0;
-}
-
-static const struct nfc_protocol llcp_nfc_proto = {
-       .id       = NFC_SOCKPROTO_LLCP,
-       .proto    = &llcp_sock_proto,
-       .owner    = THIS_MODULE,
-       .create   = llcp_sock_create
-};
-
-int __init nfc_llcp_sock_init(void)
-{
-       return nfc_proto_register(&llcp_nfc_proto);
-}
-
-void nfc_llcp_sock_exit(void)
-{
-       nfc_proto_unregister(&llcp_nfc_proto);
-}
diff --git a/net/nfc/llcp_commands.c b/net/nfc/llcp_commands.c
new file mode 100644 (file)
index 0000000..c1b23ee
--- /dev/null
@@ -0,0 +1,817 @@
+/*
+ * Copyright (C) 2011  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 as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include <net/nfc/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static u8 llcp_tlv_length[LLCP_TLV_MAX] = {
+       0,
+       1, /* VERSION */
+       2, /* MIUX */
+       2, /* WKS */
+       1, /* LTO */
+       1, /* RW */
+       0, /* SN */
+       1, /* OPT */
+       0, /* SDREQ */
+       2, /* SDRES */
+
+};
+
+static u8 llcp_tlv8(u8 *tlv, u8 type)
+{
+       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+               return 0;
+
+       return tlv[2];
+}
+
+static u16 llcp_tlv16(u8 *tlv, u8 type)
+{
+       if (tlv[0] != type || tlv[1] != llcp_tlv_length[tlv[0]])
+               return 0;
+
+       return be16_to_cpu(*((__be16 *)(tlv + 2)));
+}
+
+
+static u8 llcp_tlv_version(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_VERSION);
+}
+
+static u16 llcp_tlv_miux(u8 *tlv)
+{
+       return llcp_tlv16(tlv, LLCP_TLV_MIUX) & 0x7ff;
+}
+
+static u16 llcp_tlv_wks(u8 *tlv)
+{
+       return llcp_tlv16(tlv, LLCP_TLV_WKS);
+}
+
+static u16 llcp_tlv_lto(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_LTO);
+}
+
+static u8 llcp_tlv_opt(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_OPT);
+}
+
+static u8 llcp_tlv_rw(u8 *tlv)
+{
+       return llcp_tlv8(tlv, LLCP_TLV_RW) & 0xf;
+}
+
+u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length)
+{
+       u8 *tlv, length;
+
+       pr_debug("type %d\n", type);
+
+       if (type >= LLCP_TLV_MAX)
+               return NULL;
+
+       length = llcp_tlv_length[type];
+       if (length == 0 && value_length == 0)
+               return NULL;
+       else if (length == 0)
+               length = value_length;
+
+       *tlv_length = 2 + length;
+       tlv = kzalloc(2 + length, GFP_KERNEL);
+       if (tlv == NULL)
+               return tlv;
+
+       tlv[0] = type;
+       tlv[1] = length;
+       memcpy(tlv + 2, value, length);
+
+       return tlv;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)
+{
+       struct nfc_llcp_sdp_tlv *sdres;
+       u8 value[2];
+
+       sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+       if (sdres == NULL)
+               return NULL;
+
+       value[0] = tid;
+       value[1] = sap;
+
+       sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,
+                                       &sdres->tlv_len);
+       if (sdres->tlv == NULL) {
+               kfree(sdres);
+               return NULL;
+       }
+
+       sdres->tid = tid;
+       sdres->sap = sap;
+
+       INIT_HLIST_NODE(&sdres->node);
+
+       return sdres;
+}
+
+struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdreq_tlv(u8 tid, char *uri,
+                                                 size_t uri_len)
+{
+       struct nfc_llcp_sdp_tlv *sdreq;
+
+       pr_debug("uri: %s, len: %zu\n", uri, uri_len);
+
+       sdreq = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);
+       if (sdreq == NULL)
+               return NULL;
+
+       sdreq->tlv_len = uri_len + 3;
+
+       if (uri[uri_len - 1] == 0)
+               sdreq->tlv_len--;
+
+       sdreq->tlv = kzalloc(sdreq->tlv_len + 1, GFP_KERNEL);
+       if (sdreq->tlv == NULL) {
+               kfree(sdreq);
+               return NULL;
+       }
+
+       sdreq->tlv[0] = LLCP_TLV_SDREQ;
+       sdreq->tlv[1] = sdreq->tlv_len - 2;
+       sdreq->tlv[2] = tid;
+
+       sdreq->tid = tid;
+       sdreq->uri = sdreq->tlv + 3;
+       memcpy(sdreq->uri, uri, uri_len);
+
+       sdreq->time = jiffies;
+
+       INIT_HLIST_NODE(&sdreq->node);
+
+       return sdreq;
+}
+
+void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
+{
+       kfree(sdp->tlv);
+       kfree(sdp);
+}
+
+void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head)
+{
+       struct nfc_llcp_sdp_tlv *sdp;
+       struct hlist_node *n;
+
+       hlist_for_each_entry_safe(sdp, n, head, node) {
+               hlist_del(&sdp->node);
+
+               nfc_llcp_free_sdp_tlv(sdp);
+       }
+}
+
+int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,
+                         u8 *tlv_array, u16 tlv_array_len)
+{
+       u8 *tlv = tlv_array, type, length, offset = 0;
+
+       pr_debug("TLV array length %d\n", tlv_array_len);
+
+       if (local == NULL)
+               return -ENODEV;
+
+       while (offset < tlv_array_len) {
+               type = tlv[0];
+               length = tlv[1];
+
+               pr_debug("type 0x%x length %d\n", type, length);
+
+               switch (type) {
+               case LLCP_TLV_VERSION:
+                       local->remote_version = llcp_tlv_version(tlv);
+                       break;
+               case LLCP_TLV_MIUX:
+                       local->remote_miu = llcp_tlv_miux(tlv) + 128;
+                       break;
+               case LLCP_TLV_WKS:
+                       local->remote_wks = llcp_tlv_wks(tlv);
+                       break;
+               case LLCP_TLV_LTO:
+                       local->remote_lto = llcp_tlv_lto(tlv) * 10;
+                       break;
+               case LLCP_TLV_OPT:
+                       local->remote_opt = llcp_tlv_opt(tlv);
+                       break;
+               default:
+                       pr_err("Invalid gt tlv value 0x%x\n", type);
+                       break;
+               }
+
+               offset += length + 2;
+               tlv += length + 2;
+       }
+
+       pr_debug("version 0x%x miu %d lto %d opt 0x%x wks 0x%x\n",
+                local->remote_version, local->remote_miu,
+                local->remote_lto, local->remote_opt,
+                local->remote_wks);
+
+       return 0;
+}
+
+int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock,
+                                 u8 *tlv_array, u16 tlv_array_len)
+{
+       u8 *tlv = tlv_array, type, length, offset = 0;
+
+       pr_debug("TLV array length %d\n", tlv_array_len);
+
+       if (sock == NULL)
+               return -ENOTCONN;
+
+       while (offset < tlv_array_len) {
+               type = tlv[0];
+               length = tlv[1];
+
+               pr_debug("type 0x%x length %d\n", type, length);
+
+               switch (type) {
+               case LLCP_TLV_MIUX:
+                       sock->remote_miu = llcp_tlv_miux(tlv) + 128;
+                       break;
+               case LLCP_TLV_RW:
+                       sock->remote_rw = llcp_tlv_rw(tlv);
+                       break;
+               case LLCP_TLV_SN:
+                       break;
+               default:
+                       pr_err("Invalid gt tlv value 0x%x\n", type);
+                       break;
+               }
+
+               offset += length + 2;
+               tlv += length + 2;
+       }
+
+       pr_debug("sock %p rw %d miu %d\n", sock,
+                sock->remote_rw, sock->remote_miu);
+
+       return 0;
+}
+
+static struct sk_buff *llcp_add_header(struct sk_buff *pdu,
+                                      u8 dsap, u8 ssap, u8 ptype)
+{
+       u8 header[2];
+
+       pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+       header[0] = (u8)((dsap << 2) | (ptype >> 2));
+       header[1] = (u8)((ptype << 6) | ssap);
+
+       pr_debug("header 0x%x 0x%x\n", header[0], header[1]);
+
+       memcpy(skb_put(pdu, LLCP_HEADER_SIZE), header, LLCP_HEADER_SIZE);
+
+       return pdu;
+}
+
+static struct sk_buff *llcp_add_tlv(struct sk_buff *pdu, u8 *tlv,
+                                   u8 tlv_length)
+{
+       /* XXX Add an skb length check */
+
+       if (tlv == NULL)
+               return NULL;
+
+       memcpy(skb_put(pdu, tlv_length), tlv, tlv_length);
+
+       return pdu;
+}
+
+static struct sk_buff *llcp_allocate_pdu(struct nfc_llcp_sock *sock,
+                                        u8 cmd, u16 size)
+{
+       struct sk_buff *skb;
+       int err;
+
+       if (sock->ssap == 0)
+               return NULL;
+
+       skb = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+                                size + LLCP_HEADER_SIZE, &err);
+       if (skb == NULL) {
+               pr_err("Could not allocate PDU\n");
+               return NULL;
+       }
+
+       skb = llcp_add_header(skb, sock->dsap, sock->ssap, cmd);
+
+       return skb;
+}
+
+int nfc_llcp_disconnect(struct nfc_llcp_sock *sock)
+{
+       struct sk_buff *skb;
+       struct nfc_dev *dev;
+       struct nfc_llcp_local *local;
+
+       pr_debug("Sending DISC\n");
+
+       local = sock->local;
+       if (local == NULL)
+               return -ENODEV;
+
+       dev = sock->dev;
+       if (dev == NULL)
+               return -ENODEV;
+
+       skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
+       if (skb == NULL)
+               return -ENOMEM;
+
+       skb_queue_tail(&local->tx_queue, skb);
+
+       return 0;
+}
+
+int nfc_llcp_send_symm(struct nfc_dev *dev)
+{
+       struct sk_buff *skb;
+       struct nfc_llcp_local *local;
+       u16 size = 0;
+
+       pr_debug("Sending SYMM\n");
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL)
+               return -ENODEV;
+
+       size += LLCP_HEADER_SIZE;
+       size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+       skb = alloc_skb(size, GFP_KERNEL);
+       if (skb == NULL)
+               return -ENOMEM;
+
+       skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+       skb = llcp_add_header(skb, 0, 0, LLCP_PDU_SYMM);
+
+       __net_timestamp(skb);
+
+       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_TX);
+
+       return nfc_data_exchange(dev, local->target_idx, skb,
+                                nfc_llcp_recv, local);
+}
+
+int nfc_llcp_send_connect(struct nfc_llcp_sock *sock)
+{
+       struct nfc_llcp_local *local;
+       struct sk_buff *skb;
+       u8 *service_name_tlv = NULL, service_name_tlv_length;
+       u8 *miux_tlv = NULL, miux_tlv_length;
+       u8 *rw_tlv = NULL, rw_tlv_length, rw;
+       int err;
+       u16 size = 0, miux;
+
+       pr_debug("Sending CONNECT\n");
+
+       local = sock->local;
+       if (local == NULL)
+               return -ENODEV;
+
+       if (sock->service_name != NULL) {
+               service_name_tlv = nfc_llcp_build_tlv(LLCP_TLV_SN,
+                                                     sock->service_name,
+                                                     sock->service_name_len,
+                                                     &service_name_tlv_length);
+               size += service_name_tlv_length;
+       }
+
+       /* If the socket parameters are not set, use the local ones */
+       miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
+               local->miux : sock->miux;
+       rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
+
+       miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+                                     &miux_tlv_length);
+       size += miux_tlv_length;
+
+       rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+       size += rw_tlv_length;
+
+       pr_debug("SKB size %d SN length %zu\n", size, sock->service_name_len);
+
+       skb = llcp_allocate_pdu(sock, LLCP_PDU_CONNECT, size);
+       if (skb == NULL) {
+               err = -ENOMEM;
+               goto error_tlv;
+       }
+
+       if (service_name_tlv != NULL)
+               skb = llcp_add_tlv(skb, service_name_tlv,
+                                  service_name_tlv_length);
+
+       skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+       skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+       skb_queue_tail(&local->tx_queue, skb);
+
+       return 0;
+
+error_tlv:
+       pr_err("error %d\n", err);
+
+       kfree(service_name_tlv);
+       kfree(miux_tlv);
+       kfree(rw_tlv);
+
+       return err;
+}
+
+int nfc_llcp_send_cc(struct nfc_llcp_sock *sock)
+{
+       struct nfc_llcp_local *local;
+       struct sk_buff *skb;
+       u8 *miux_tlv = NULL, miux_tlv_length;
+       u8 *rw_tlv = NULL, rw_tlv_length, rw;
+       int err;
+       u16 size = 0, miux;
+
+       pr_debug("Sending CC\n");
+
+       local = sock->local;
+       if (local == NULL)
+               return -ENODEV;
+
+       /* If the socket parameters are not set, use the local ones */
+       miux = be16_to_cpu(sock->miux) > LLCP_MAX_MIUX ?
+               local->miux : sock->miux;
+       rw = sock->rw > LLCP_MAX_RW ? local->rw : sock->rw;
+
+       miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&miux, 0,
+                                     &miux_tlv_length);
+       size += miux_tlv_length;
+
+       rw_tlv = nfc_llcp_build_tlv(LLCP_TLV_RW, &rw, 0, &rw_tlv_length);
+       size += rw_tlv_length;
+
+       skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, size);
+       if (skb == NULL) {
+               err = -ENOMEM;
+               goto error_tlv;
+       }
+
+       skb = llcp_add_tlv(skb, miux_tlv, miux_tlv_length);
+       skb = llcp_add_tlv(skb, rw_tlv, rw_tlv_length);
+
+       skb_queue_tail(&local->tx_queue, skb);
+
+       return 0;
+
+error_tlv:
+       pr_err("error %d\n", err);
+
+       kfree(miux_tlv);
+       kfree(rw_tlv);
+
+       return err;
+}
+
+static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,
+                                            size_t tlv_length)
+{
+       struct sk_buff *skb;
+       struct nfc_dev *dev;
+       u16 size = 0;
+
+       if (local == NULL)
+               return ERR_PTR(-ENODEV);
+
+       dev = local->dev;
+       if (dev == NULL)
+               return ERR_PTR(-ENODEV);
+
+       size += LLCP_HEADER_SIZE;
+       size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+       size += tlv_length;
+
+       skb = alloc_skb(size, GFP_KERNEL);
+       if (skb == NULL)
+               return ERR_PTR(-ENOMEM);
+
+       skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+       skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);
+
+       return skb;
+}
+
+int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,
+                           struct hlist_head *tlv_list, size_t tlvs_len)
+{
+       struct nfc_llcp_sdp_tlv *sdp;
+       struct hlist_node *n;
+       struct sk_buff *skb;
+
+       skb = nfc_llcp_allocate_snl(local, tlvs_len);
+       if (IS_ERR(skb))
+               return PTR_ERR(skb);
+
+       hlist_for_each_entry_safe(sdp, n, tlv_list, node) {
+               memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len);
+
+               hlist_del(&sdp->node);
+
+               nfc_llcp_free_sdp_tlv(sdp);
+       }
+
+       skb_queue_tail(&local->tx_queue, skb);
+
+       return 0;
+}
+
+int nfc_llcp_send_snl_sdreq(struct nfc_llcp_local *local,
+                           struct hlist_head *tlv_list, size_t tlvs_len)
+{
+       struct nfc_llcp_sdp_tlv *sdreq;
+       struct hlist_node *n;
+       struct sk_buff *skb;
+
+       skb = nfc_llcp_allocate_snl(local, tlvs_len);
+       if (IS_ERR(skb))
+               return PTR_ERR(skb);
+
+       mutex_lock(&local->sdreq_lock);
+
+       if (hlist_empty(&local->pending_sdreqs))
+               mod_timer(&local->sdreq_timer,
+                         jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
+       hlist_for_each_entry_safe(sdreq, n, tlv_list, node) {
+               pr_debug("tid %d for %s\n", sdreq->tid, sdreq->uri);
+
+               memcpy(skb_put(skb, sdreq->tlv_len), sdreq->tlv,
+                      sdreq->tlv_len);
+
+               hlist_del(&sdreq->node);
+
+               hlist_add_head(&sdreq->node, &local->pending_sdreqs);
+       }
+
+       mutex_unlock(&local->sdreq_lock);
+
+       skb_queue_tail(&local->tx_queue, skb);
+
+       return 0;
+}
+
+int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason)
+{
+       struct sk_buff *skb;
+       struct nfc_dev *dev;
+       u16 size = 1; /* Reason code */
+
+       pr_debug("Sending DM reason 0x%x\n", reason);
+
+       if (local == NULL)
+               return -ENODEV;
+
+       dev = local->dev;
+       if (dev == NULL)
+               return -ENODEV;
+
+       size += LLCP_HEADER_SIZE;
+       size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;
+
+       skb = alloc_skb(size, GFP_KERNEL);
+       if (skb == NULL)
+               return -ENOMEM;
+
+       skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);
+
+       skb = llcp_add_header(skb, dsap, ssap, LLCP_PDU_DM);
+
+       memcpy(skb_put(skb, 1), &reason, 1);
+
+       skb_queue_head(&local->tx_queue, skb);
+
+       return 0;
+}
+
+int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock)
+{
+       struct sk_buff *skb;
+       struct nfc_llcp_local *local;
+
+       pr_debug("Send DISC\n");
+
+       local = sock->local;
+       if (local == NULL)
+               return -ENODEV;
+
+       skb = llcp_allocate_pdu(sock, LLCP_PDU_DISC, 0);
+       if (skb == NULL)
+               return -ENOMEM;
+
+       skb_queue_head(&local->tx_queue, skb);
+
+       return 0;
+}
+
+int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,
+                         struct msghdr *msg, size_t len)
+{
+       struct sk_buff *pdu;
+       struct sock *sk = &sock->sk;
+       struct nfc_llcp_local *local;
+       size_t frag_len = 0, remaining_len;
+       u8 *msg_data, *msg_ptr;
+       u16 remote_miu;
+
+       pr_debug("Send I frame len %zd\n", len);
+
+       local = sock->local;
+       if (local == NULL)
+               return -ENODEV;
+
+       /* Remote is ready but has not acknowledged our frames */
+       if((sock->remote_ready &&
+           skb_queue_len(&sock->tx_pending_queue) >= sock->remote_rw &&
+           skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
+               pr_err("Pending queue is full %d frames\n",
+                      skb_queue_len(&sock->tx_pending_queue));
+               return -ENOBUFS;
+       }
+
+       /* Remote is not ready and we've been queueing enough frames */
+       if ((!sock->remote_ready &&
+            skb_queue_len(&sock->tx_queue) >= 2 * sock->remote_rw)) {
+               pr_err("Tx queue is full %d frames\n",
+                      skb_queue_len(&sock->tx_queue));
+               return -ENOBUFS;
+       }
+
+       msg_data = kzalloc(len, GFP_KERNEL);
+       if (msg_data == NULL)
+               return -ENOMEM;
+
+       if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+               kfree(msg_data);
+               return -EFAULT;
+       }
+
+       remaining_len = len;
+       msg_ptr = msg_data;
+
+       do {
+               remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
+                               local->remote_miu : sock->remote_miu;
+
+               frag_len = min_t(size_t, remote_miu, remaining_len);
+
+               pr_debug("Fragment %zd bytes remaining %zd",
+                        frag_len, remaining_len);
+
+               pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
+                                       frag_len + LLCP_SEQUENCE_SIZE);
+               if (pdu == NULL)
+                       return -ENOMEM;
+
+               skb_put(pdu, LLCP_SEQUENCE_SIZE);
+
+               if (likely(frag_len > 0))
+                       memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+               skb_queue_tail(&sock->tx_queue, pdu);
+
+               lock_sock(sk);
+
+               nfc_llcp_queue_i_frames(sock);
+
+               release_sock(sk);
+
+               remaining_len -= frag_len;
+               msg_ptr += frag_len;
+       } while (remaining_len > 0);
+
+       kfree(msg_data);
+
+       return len;
+}
+
+int nfc_llcp_send_ui_frame(struct nfc_llcp_sock *sock, u8 ssap, u8 dsap,
+                          struct msghdr *msg, size_t len)
+{
+       struct sk_buff *pdu;
+       struct nfc_llcp_local *local;
+       size_t frag_len = 0, remaining_len;
+       u8 *msg_ptr, *msg_data;
+       u16 remote_miu;
+       int err;
+
+       pr_debug("Send UI frame len %zd\n", len);
+
+       local = sock->local;
+       if (local == NULL)
+               return -ENODEV;
+
+       msg_data = kzalloc(len, GFP_KERNEL);
+       if (msg_data == NULL)
+               return -ENOMEM;
+
+       if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+               kfree(msg_data);
+               return -EFAULT;
+       }
+
+       remaining_len = len;
+       msg_ptr = msg_data;
+
+       do {
+               remote_miu = sock->remote_miu > LLCP_MAX_MIU ?
+                               local->remote_miu : sock->remote_miu;
+
+               frag_len = min_t(size_t, remote_miu, remaining_len);
+
+               pr_debug("Fragment %zd bytes remaining %zd",
+                        frag_len, remaining_len);
+
+               pdu = nfc_alloc_send_skb(sock->dev, &sock->sk, MSG_DONTWAIT,
+                                        frag_len + LLCP_HEADER_SIZE, &err);
+               if (pdu == NULL) {
+                       pr_err("Could not allocate PDU\n");
+                       continue;
+               }
+
+               pdu = llcp_add_header(pdu, dsap, ssap, LLCP_PDU_UI);
+
+               if (likely(frag_len > 0))
+                       memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+               /* No need to check for the peer RW for UI frames */
+               skb_queue_tail(&local->tx_queue, pdu);
+
+               remaining_len -= frag_len;
+               msg_ptr += frag_len;
+       } while (remaining_len > 0);
+
+       kfree(msg_data);
+
+       return len;
+}
+
+int nfc_llcp_send_rr(struct nfc_llcp_sock *sock)
+{
+       struct sk_buff *skb;
+       struct nfc_llcp_local *local;
+
+       pr_debug("Send rr nr %d\n", sock->recv_n);
+
+       local = sock->local;
+       if (local == NULL)
+               return -ENODEV;
+
+       skb = llcp_allocate_pdu(sock, LLCP_PDU_RR, LLCP_SEQUENCE_SIZE);
+       if (skb == NULL)
+               return -ENOMEM;
+
+       skb_put(skb, LLCP_SEQUENCE_SIZE);
+
+       skb->data[2] = sock->recv_n;
+
+       skb_queue_head(&local->tx_queue, skb);
+
+       return 0;
+}
diff --git a/net/nfc/llcp_core.c b/net/nfc/llcp_core.c
new file mode 100644 (file)
index 0000000..158bdbf
--- /dev/null
@@ -0,0 +1,1624 @@
+/*
+ * Copyright (C) 2011  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 as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
+
+static struct list_head llcp_devices;
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
+
+void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
+{
+       write_lock(&l->lock);
+       sk_add_node(sk, &l->head);
+       write_unlock(&l->lock);
+}
+
+void nfc_llcp_sock_unlink(struct llcp_sock_list *l, struct sock *sk)
+{
+       write_lock(&l->lock);
+       sk_del_node_init(sk);
+       write_unlock(&l->lock);
+}
+
+void nfc_llcp_socket_remote_param_init(struct nfc_llcp_sock *sock)
+{
+       sock->remote_rw = LLCP_DEFAULT_RW;
+       sock->remote_miu = LLCP_MAX_MIU + 1;
+}
+
+static void nfc_llcp_socket_purge(struct nfc_llcp_sock *sock)
+{
+       struct nfc_llcp_local *local = sock->local;
+       struct sk_buff *s, *tmp;
+
+       pr_debug("%p\n", &sock->sk);
+
+       skb_queue_purge(&sock->tx_queue);
+       skb_queue_purge(&sock->tx_pending_queue);
+
+       if (local == NULL)
+               return;
+
+       /* Search for local pending SKBs that are related to this socket */
+       skb_queue_walk_safe(&local->tx_queue, s, tmp) {
+               if (s->sk != &sock->sk)
+                       continue;
+
+               skb_unlink(s, &local->tx_queue);
+               kfree_skb(s);
+       }
+}
+
+static void nfc_llcp_socket_release(struct nfc_llcp_local *local, bool device,
+                                   int err)
+{
+       struct sock *sk;
+       struct hlist_node *tmp;
+       struct nfc_llcp_sock *llcp_sock;
+
+       skb_queue_purge(&local->tx_queue);
+
+       write_lock(&local->sockets.lock);
+
+       sk_for_each_safe(sk, tmp, &local->sockets.head) {
+               llcp_sock = nfc_llcp_sock(sk);
+
+               bh_lock_sock(sk);
+
+               nfc_llcp_socket_purge(llcp_sock);
+
+               if (sk->sk_state == LLCP_CONNECTED)
+                       nfc_put_device(llcp_sock->dev);
+
+               if (sk->sk_state == LLCP_LISTEN) {
+                       struct nfc_llcp_sock *lsk, *n;
+                       struct sock *accept_sk;
+
+                       list_for_each_entry_safe(lsk, n,
+                                                &llcp_sock->accept_queue,
+                                                accept_queue) {
+                               accept_sk = &lsk->sk;
+                               bh_lock_sock(accept_sk);
+
+                               nfc_llcp_accept_unlink(accept_sk);
+
+                               if (err)
+                                       accept_sk->sk_err = err;
+                               accept_sk->sk_state = LLCP_CLOSED;
+                               accept_sk->sk_state_change(sk);
+
+                               bh_unlock_sock(accept_sk);
+                       }
+               }
+
+               if (err)
+                       sk->sk_err = err;
+               sk->sk_state = LLCP_CLOSED;
+               sk->sk_state_change(sk);
+
+               bh_unlock_sock(sk);
+
+               sk_del_node_init(sk);
+       }
+
+       write_unlock(&local->sockets.lock);
+
+       /* If we still have a device, we keep the RAW sockets alive */
+       if (device == true)
+               return;
+
+       write_lock(&local->raw_sockets.lock);
+
+       sk_for_each_safe(sk, tmp, &local->raw_sockets.head) {
+               llcp_sock = nfc_llcp_sock(sk);
+
+               bh_lock_sock(sk);
+
+               nfc_llcp_socket_purge(llcp_sock);
+
+               if (err)
+                       sk->sk_err = err;
+               sk->sk_state = LLCP_CLOSED;
+               sk->sk_state_change(sk);
+
+               bh_unlock_sock(sk);
+
+               sk_del_node_init(sk);
+       }
+
+       write_unlock(&local->raw_sockets.lock);
+}
+
+struct nfc_llcp_local *nfc_llcp_local_get(struct nfc_llcp_local *local)
+{
+       kref_get(&local->ref);
+
+       return local;
+}
+
+static void local_cleanup(struct nfc_llcp_local *local)
+{
+       nfc_llcp_socket_release(local, false, ENXIO);
+       del_timer_sync(&local->link_timer);
+       skb_queue_purge(&local->tx_queue);
+       cancel_work_sync(&local->tx_work);
+       cancel_work_sync(&local->rx_work);
+       cancel_work_sync(&local->timeout_work);
+       kfree_skb(local->rx_pending);
+       del_timer_sync(&local->sdreq_timer);
+       cancel_work_sync(&local->sdreq_timeout_work);
+       nfc_llcp_free_sdp_tlv_list(&local->pending_sdreqs);
+}
+
+static void local_release(struct kref *ref)
+{
+       struct nfc_llcp_local *local;
+
+       local = container_of(ref, struct nfc_llcp_local, ref);
+
+       list_del(&local->list);
+       local_cleanup(local);
+       kfree(local);
+}
+
+int nfc_llcp_local_put(struct nfc_llcp_local *local)
+{
+       if (local == NULL)
+               return 0;
+
+       return kref_put(&local->ref, local_release);
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local,
+                                              u8 ssap, u8 dsap)
+{
+       struct sock *sk;
+       struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+       pr_debug("ssap dsap %d %d\n", ssap, dsap);
+
+       if (ssap == 0 && dsap == 0)
+               return NULL;
+
+       read_lock(&local->sockets.lock);
+
+       llcp_sock = NULL;
+
+       sk_for_each(sk, &local->sockets.head) {
+               tmp_sock = nfc_llcp_sock(sk);
+
+               if (tmp_sock->ssap == ssap && tmp_sock->dsap == dsap) {
+                       llcp_sock = tmp_sock;
+                       break;
+               }
+       }
+
+       read_unlock(&local->sockets.lock);
+
+       if (llcp_sock == NULL)
+               return NULL;
+
+       sock_hold(&llcp_sock->sk);
+
+       return llcp_sock;
+}
+
+static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock)
+{
+       sock_put(&sock->sk);
+}
+
+static void nfc_llcp_timeout_work(struct work_struct *work)
+{
+       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                   timeout_work);
+
+       nfc_dep_link_down(local->dev);
+}
+
+static void nfc_llcp_symm_timer(unsigned long data)
+{
+       struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+       pr_err("SYMM timeout\n");
+
+       schedule_work(&local->timeout_work);
+}
+
+static void nfc_llcp_sdreq_timeout_work(struct work_struct *work)
+{
+       unsigned long time;
+       HLIST_HEAD(nl_sdres_list);
+       struct hlist_node *n;
+       struct nfc_llcp_sdp_tlv *sdp;
+       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                   sdreq_timeout_work);
+
+       mutex_lock(&local->sdreq_lock);
+
+       time = jiffies - msecs_to_jiffies(3 * local->remote_lto);
+
+       hlist_for_each_entry_safe(sdp, n, &local->pending_sdreqs, node) {
+               if (time_after(sdp->time, time))
+                       continue;
+
+               sdp->sap = LLCP_SDP_UNBOUND;
+
+               hlist_del(&sdp->node);
+
+               hlist_add_head(&sdp->node, &nl_sdres_list);
+       }
+
+       if (!hlist_empty(&local->pending_sdreqs))
+               mod_timer(&local->sdreq_timer,
+                         jiffies + msecs_to_jiffies(3 * local->remote_lto));
+
+       mutex_unlock(&local->sdreq_lock);
+
+       if (!hlist_empty(&nl_sdres_list))
+               nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+}
+
+static void nfc_llcp_sdreq_timer(unsigned long data)
+{
+       struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+       schedule_work(&local->sdreq_timeout_work);
+}
+
+struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
+{
+       struct nfc_llcp_local *local, *n;
+
+       list_for_each_entry_safe(local, n, &llcp_devices, list)
+               if (local->dev == dev)
+                       return local;
+
+       pr_debug("No device found\n");
+
+       return NULL;
+}
+
+static char *wks[] = {
+       NULL,
+       NULL, /* SDP */
+       "urn:nfc:sn:ip",
+       "urn:nfc:sn:obex",
+       "urn:nfc:sn:snep",
+};
+
+static int nfc_llcp_wks_sap(char *service_name, size_t service_name_len)
+{
+       int sap, num_wks;
+
+       pr_debug("%s\n", service_name);
+
+       if (service_name == NULL)
+               return -EINVAL;
+
+       num_wks = ARRAY_SIZE(wks);
+
+       for (sap = 0; sap < num_wks; sap++) {
+               if (wks[sap] == NULL)
+                       continue;
+
+               if (strncmp(wks[sap], service_name, service_name_len) == 0)
+                       return sap;
+       }
+
+       return -EINVAL;
+}
+
+static
+struct nfc_llcp_sock *nfc_llcp_sock_from_sn(struct nfc_llcp_local *local,
+                                           u8 *sn, size_t sn_len)
+{
+       struct sock *sk;
+       struct nfc_llcp_sock *llcp_sock, *tmp_sock;
+
+       pr_debug("sn %zd %p\n", sn_len, sn);
+
+       if (sn == NULL || sn_len == 0)
+               return NULL;
+
+       read_lock(&local->sockets.lock);
+
+       llcp_sock = NULL;
+
+       sk_for_each(sk, &local->sockets.head) {
+               tmp_sock = nfc_llcp_sock(sk);
+
+               pr_debug("llcp sock %p\n", tmp_sock);
+
+               if (tmp_sock->sk.sk_type == SOCK_STREAM &&
+                   tmp_sock->sk.sk_state != LLCP_LISTEN)
+                       continue;
+
+               if (tmp_sock->sk.sk_type == SOCK_DGRAM &&
+                   tmp_sock->sk.sk_state != LLCP_BOUND)
+                       continue;
+
+               if (tmp_sock->service_name == NULL ||
+                   tmp_sock->service_name_len == 0)
+                       continue;
+
+               if (tmp_sock->service_name_len != sn_len)
+                       continue;
+
+               if (memcmp(sn, tmp_sock->service_name, sn_len) == 0) {
+                       llcp_sock = tmp_sock;
+                       break;
+               }
+       }
+
+       read_unlock(&local->sockets.lock);
+
+       pr_debug("Found llcp sock %p\n", llcp_sock);
+
+       return llcp_sock;
+}
+
+u8 nfc_llcp_get_sdp_ssap(struct nfc_llcp_local *local,
+                        struct nfc_llcp_sock *sock)
+{
+       mutex_lock(&local->sdp_lock);
+
+       if (sock->service_name != NULL && sock->service_name_len > 0) {
+               int ssap = nfc_llcp_wks_sap(sock->service_name,
+                                           sock->service_name_len);
+
+               if (ssap > 0) {
+                       pr_debug("WKS %d\n", ssap);
+
+                       /* This is a WKS, let's check if it's free */
+                       if (local->local_wks & BIT(ssap)) {
+                               mutex_unlock(&local->sdp_lock);
+
+                               return LLCP_SAP_MAX;
+                       }
+
+                       set_bit(ssap, &local->local_wks);
+                       mutex_unlock(&local->sdp_lock);
+
+                       return ssap;
+               }
+
+               /*
+                * Check if there already is a non WKS socket bound
+                * to this service name.
+                */
+               if (nfc_llcp_sock_from_sn(local, sock->service_name,
+                                         sock->service_name_len) != NULL) {
+                       mutex_unlock(&local->sdp_lock);
+
+                       return LLCP_SAP_MAX;
+               }
+
+               mutex_unlock(&local->sdp_lock);
+
+               return LLCP_SDP_UNBOUND;
+
+       } else if (sock->ssap != 0 && sock->ssap < LLCP_WKS_NUM_SAP) {
+               if (!test_bit(sock->ssap, &local->local_wks)) {
+                       set_bit(sock->ssap, &local->local_wks);
+                       mutex_unlock(&local->sdp_lock);
+
+                       return sock->ssap;
+               }
+       }
+
+       mutex_unlock(&local->sdp_lock);
+
+       return LLCP_SAP_MAX;
+}
+
+u8 nfc_llcp_get_local_ssap(struct nfc_llcp_local *local)
+{
+       u8 local_ssap;
+
+       mutex_lock(&local->sdp_lock);
+
+       local_ssap = find_first_zero_bit(&local->local_sap, LLCP_LOCAL_NUM_SAP);
+       if (local_ssap == LLCP_LOCAL_NUM_SAP) {
+               mutex_unlock(&local->sdp_lock);
+               return LLCP_SAP_MAX;
+       }
+
+       set_bit(local_ssap, &local->local_sap);
+
+       mutex_unlock(&local->sdp_lock);
+
+       return local_ssap + LLCP_LOCAL_SAP_OFFSET;
+}
+
+void nfc_llcp_put_ssap(struct nfc_llcp_local *local, u8 ssap)
+{
+       u8 local_ssap;
+       unsigned long *sdp;
+
+       if (ssap < LLCP_WKS_NUM_SAP) {
+               local_ssap = ssap;
+               sdp = &local->local_wks;
+       } else if (ssap < LLCP_LOCAL_NUM_SAP) {
+               atomic_t *client_cnt;
+
+               local_ssap = ssap - LLCP_WKS_NUM_SAP;
+               sdp = &local->local_sdp;
+               client_cnt = &local->local_sdp_cnt[local_ssap];
+
+               pr_debug("%d clients\n", atomic_read(client_cnt));
+
+               mutex_lock(&local->sdp_lock);
+
+               if (atomic_dec_and_test(client_cnt)) {
+                       struct nfc_llcp_sock *l_sock;
+
+                       pr_debug("No more clients for SAP %d\n", ssap);
+
+                       clear_bit(local_ssap, sdp);
+
+                       /* Find the listening sock and set it back to UNBOUND */
+                       l_sock = nfc_llcp_sock_get(local, ssap, LLCP_SAP_SDP);
+                       if (l_sock) {
+                               l_sock->ssap = LLCP_SDP_UNBOUND;
+                               nfc_llcp_sock_put(l_sock);
+                       }
+               }
+
+               mutex_unlock(&local->sdp_lock);
+
+               return;
+       } else if (ssap < LLCP_MAX_SAP) {
+               local_ssap = ssap - LLCP_LOCAL_NUM_SAP;
+               sdp = &local->local_sap;
+       } else {
+               return;
+       }
+
+       mutex_lock(&local->sdp_lock);
+
+       clear_bit(local_ssap, sdp);
+
+       mutex_unlock(&local->sdp_lock);
+}
+
+static u8 nfc_llcp_reserve_sdp_ssap(struct nfc_llcp_local *local)
+{
+       u8 ssap;
+
+       mutex_lock(&local->sdp_lock);
+
+       ssap = find_first_zero_bit(&local->local_sdp, LLCP_SDP_NUM_SAP);
+       if (ssap == LLCP_SDP_NUM_SAP) {
+               mutex_unlock(&local->sdp_lock);
+
+               return LLCP_SAP_MAX;
+       }
+
+       pr_debug("SDP ssap %d\n", LLCP_WKS_NUM_SAP + ssap);
+
+       set_bit(ssap, &local->local_sdp);
+
+       mutex_unlock(&local->sdp_lock);
+
+       return LLCP_WKS_NUM_SAP + ssap;
+}
+
+static int nfc_llcp_build_gb(struct nfc_llcp_local *local)
+{
+       u8 *gb_cur, *version_tlv, version, version_length;
+       u8 *lto_tlv, lto_length;
+       u8 *wks_tlv, wks_length;
+       u8 *miux_tlv, miux_length;
+       u8 gb_len = 0;
+       int ret = 0;
+
+       version = LLCP_VERSION_11;
+       version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version,
+                                        1, &version_length);
+       gb_len += version_length;
+
+       lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_LTO, &local->lto, 1, &lto_length);
+       gb_len += lto_length;
+
+       pr_debug("Local wks 0x%lx\n", local->local_wks);
+       wks_tlv = nfc_llcp_build_tlv(LLCP_TLV_WKS, (u8 *)&local->local_wks, 2,
+                                    &wks_length);
+       gb_len += wks_length;
+
+       miux_tlv = nfc_llcp_build_tlv(LLCP_TLV_MIUX, (u8 *)&local->miux, 0,
+                                     &miux_length);
+       gb_len += miux_length;
+
+       gb_len += ARRAY_SIZE(llcp_magic);
+
+       if (gb_len > NFC_MAX_GT_LEN) {
+               ret = -EINVAL;
+               goto out;
+       }
+
+       gb_cur = local->gb;
+
+       memcpy(gb_cur, llcp_magic, ARRAY_SIZE(llcp_magic));
+       gb_cur += ARRAY_SIZE(llcp_magic);
+
+       memcpy(gb_cur, version_tlv, version_length);
+       gb_cur += version_length;
+
+       memcpy(gb_cur, lto_tlv, lto_length);
+       gb_cur += lto_length;
+
+       memcpy(gb_cur, wks_tlv, wks_length);
+       gb_cur += wks_length;
+
+       memcpy(gb_cur, miux_tlv, miux_length);
+       gb_cur += miux_length;
+
+       local->gb_len = gb_len;
+
+out:
+       kfree(version_tlv);
+       kfree(lto_tlv);
+       kfree(wks_tlv);
+       kfree(miux_tlv);
+
+       return ret;
+}
+
+u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *general_bytes_len)
+{
+       struct nfc_llcp_local *local;
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL) {
+               *general_bytes_len = 0;
+               return NULL;
+       }
+
+       nfc_llcp_build_gb(local);
+
+       *general_bytes_len = local->gb_len;
+
+       return local->gb;
+}
+
+int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len)
+{
+       struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
+
+       if (local == NULL) {
+               pr_err("No LLCP device\n");
+               return -ENODEV;
+       }
+       if (gb_len < 3)
+               return -EINVAL;
+
+       memset(local->remote_gb, 0, NFC_MAX_GT_LEN);
+       memcpy(local->remote_gb, gb, gb_len);
+       local->remote_gb_len = gb_len;
+
+       if (memcmp(local->remote_gb, llcp_magic, 3)) {
+               pr_err("MAC does not support LLCP\n");
+               return -EINVAL;
+       }
+
+       return nfc_llcp_parse_gb_tlv(local,
+                                    &local->remote_gb[3],
+                                    local->remote_gb_len - 3);
+}
+
+static u8 nfc_llcp_dsap(struct sk_buff *pdu)
+{
+       return (pdu->data[0] & 0xfc) >> 2;
+}
+
+static u8 nfc_llcp_ptype(struct sk_buff *pdu)
+{
+       return ((pdu->data[0] & 0x03) << 2) | ((pdu->data[1] & 0xc0) >> 6);
+}
+
+static u8 nfc_llcp_ssap(struct sk_buff *pdu)
+{
+       return pdu->data[1] & 0x3f;
+}
+
+static u8 nfc_llcp_ns(struct sk_buff *pdu)
+{
+       return pdu->data[2] >> 4;
+}
+
+static u8 nfc_llcp_nr(struct sk_buff *pdu)
+{
+       return pdu->data[2] & 0xf;
+}
+
+static void nfc_llcp_set_nrns(struct nfc_llcp_sock *sock, struct sk_buff *pdu)
+{
+       pdu->data[2] = (sock->send_n << 4) | (sock->recv_n);
+       sock->send_n = (sock->send_n + 1) % 16;
+       sock->recv_ack_n = (sock->recv_n - 1) % 16;
+}
+
+void nfc_llcp_send_to_raw_sock(struct nfc_llcp_local *local,
+                              struct sk_buff *skb, u8 direction)
+{
+       struct sk_buff *skb_copy = NULL, *nskb;
+       struct sock *sk;
+       u8 *data;
+
+       read_lock(&local->raw_sockets.lock);
+
+       sk_for_each(sk, &local->raw_sockets.head) {
+               if (sk->sk_state != LLCP_BOUND)
+                       continue;
+
+               if (skb_copy == NULL) {
+                       skb_copy = __pskb_copy(skb, NFC_LLCP_RAW_HEADER_SIZE,
+                                              GFP_ATOMIC);
+
+                       if (skb_copy == NULL)
+                               continue;
+
+                       data = skb_push(skb_copy, NFC_LLCP_RAW_HEADER_SIZE);
+
+                       data[0] = local->dev ? local->dev->idx : 0xFF;
+                       data[1] = direction;
+               }
+
+               nskb = skb_clone(skb_copy, GFP_ATOMIC);
+               if (!nskb)
+                       continue;
+
+               if (sock_queue_rcv_skb(sk, nskb))
+                       kfree_skb(nskb);
+       }
+
+       read_unlock(&local->raw_sockets.lock);
+
+       kfree_skb(skb_copy);
+}
+
+static void nfc_llcp_tx_work(struct work_struct *work)
+{
+       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                   tx_work);
+       struct sk_buff *skb;
+       struct sock *sk;
+       struct nfc_llcp_sock *llcp_sock;
+
+       skb = skb_dequeue(&local->tx_queue);
+       if (skb != NULL) {
+               sk = skb->sk;
+               llcp_sock = nfc_llcp_sock(sk);
+
+               if (llcp_sock == NULL && nfc_llcp_ptype(skb) == LLCP_PDU_I) {
+                       nfc_llcp_send_symm(local->dev);
+               } else {
+                       struct sk_buff *copy_skb = NULL;
+                       u8 ptype = nfc_llcp_ptype(skb);
+                       int ret;
+
+                       pr_debug("Sending pending skb\n");
+                       print_hex_dump(KERN_DEBUG, "LLCP Tx: ",
+                                      DUMP_PREFIX_OFFSET, 16, 1,
+                                      skb->data, skb->len, true);
+
+                       if (ptype == LLCP_PDU_I)
+                               copy_skb = skb_copy(skb, GFP_ATOMIC);
+
+                       __net_timestamp(skb);
+
+                       nfc_llcp_send_to_raw_sock(local, skb,
+                                                 NFC_LLCP_DIRECTION_TX);
+
+                       ret = nfc_data_exchange(local->dev, local->target_idx,
+                                               skb, nfc_llcp_recv, local);
+
+                       if (ret) {
+                               kfree_skb(copy_skb);
+                               goto out;
+                       }
+
+                       if (ptype == LLCP_PDU_I && copy_skb)
+                               skb_queue_tail(&llcp_sock->tx_pending_queue,
+                                              copy_skb);
+               }
+       } else {
+               nfc_llcp_send_symm(local->dev);
+       }
+
+out:
+       mod_timer(&local->link_timer,
+                 jiffies + msecs_to_jiffies(2 * local->remote_lto));
+}
+
+static struct nfc_llcp_sock *nfc_llcp_connecting_sock_get(struct nfc_llcp_local *local,
+                                                         u8 ssap)
+{
+       struct sock *sk;
+       struct nfc_llcp_sock *llcp_sock;
+
+       read_lock(&local->connecting_sockets.lock);
+
+       sk_for_each(sk, &local->connecting_sockets.head) {
+               llcp_sock = nfc_llcp_sock(sk);
+
+               if (llcp_sock->ssap == ssap) {
+                       sock_hold(&llcp_sock->sk);
+                       goto out;
+               }
+       }
+
+       llcp_sock = NULL;
+
+out:
+       read_unlock(&local->connecting_sockets.lock);
+
+       return llcp_sock;
+}
+
+static struct nfc_llcp_sock *nfc_llcp_sock_get_sn(struct nfc_llcp_local *local,
+                                                 u8 *sn, size_t sn_len)
+{
+       struct nfc_llcp_sock *llcp_sock;
+
+       llcp_sock = nfc_llcp_sock_from_sn(local, sn, sn_len);
+
+       if (llcp_sock == NULL)
+               return NULL;
+
+       sock_hold(&llcp_sock->sk);
+
+       return llcp_sock;
+}
+
+static u8 *nfc_llcp_connect_sn(struct sk_buff *skb, size_t *sn_len)
+{
+       u8 *tlv = &skb->data[2], type, length;
+       size_t tlv_array_len = skb->len - LLCP_HEADER_SIZE, offset = 0;
+
+       while (offset < tlv_array_len) {
+               type = tlv[0];
+               length = tlv[1];
+
+               pr_debug("type 0x%x length %d\n", type, length);
+
+               if (type == LLCP_TLV_SN) {
+                       *sn_len = length;
+                       return &tlv[2];
+               }
+
+               offset += length + 2;
+               tlv += length + 2;
+       }
+
+       return NULL;
+}
+
+static void nfc_llcp_recv_ui(struct nfc_llcp_local *local,
+                            struct sk_buff *skb)
+{
+       struct nfc_llcp_sock *llcp_sock;
+       struct nfc_llcp_ui_cb *ui_cb;
+       u8 dsap, ssap;
+
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+
+       ui_cb = nfc_llcp_ui_skb_cb(skb);
+       ui_cb->dsap = dsap;
+       ui_cb->ssap = ssap;
+
+       pr_debug("%d %d\n", dsap, ssap);
+
+       /* We're looking for a bound socket, not a client one */
+       llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+       if (llcp_sock == NULL || llcp_sock->sk.sk_type != SOCK_DGRAM)
+               return;
+
+       /* There is no sequence with UI frames */
+       skb_pull(skb, LLCP_HEADER_SIZE);
+       if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+               /*
+                * UI frames will be freed from the socket layer, so we
+                * need to keep them alive until someone receives them.
+                */
+               skb_get(skb);
+       } else {
+               pr_err("Receive queue is full\n");
+       }
+
+       nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_connect(struct nfc_llcp_local *local,
+                                 struct sk_buff *skb)
+{
+       struct sock *new_sk, *parent;
+       struct nfc_llcp_sock *sock, *new_sock;
+       u8 dsap, ssap, reason;
+
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+
+       pr_debug("%d %d\n", dsap, ssap);
+
+       if (dsap != LLCP_SAP_SDP) {
+               sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP);
+               if (sock == NULL || sock->sk.sk_state != LLCP_LISTEN) {
+                       reason = LLCP_DM_NOBOUND;
+                       goto fail;
+               }
+       } else {
+               u8 *sn;
+               size_t sn_len;
+
+               sn = nfc_llcp_connect_sn(skb, &sn_len);
+               if (sn == NULL) {
+                       reason = LLCP_DM_NOBOUND;
+                       goto fail;
+               }
+
+               pr_debug("Service name length %zu\n", sn_len);
+
+               sock = nfc_llcp_sock_get_sn(local, sn, sn_len);
+               if (sock == NULL) {
+                       reason = LLCP_DM_NOBOUND;
+                       goto fail;
+               }
+       }
+
+       lock_sock(&sock->sk);
+
+       parent = &sock->sk;
+
+       if (sk_acceptq_is_full(parent)) {
+               reason = LLCP_DM_REJ;
+               release_sock(&sock->sk);
+               sock_put(&sock->sk);
+               goto fail;
+       }
+
+       if (sock->ssap == LLCP_SDP_UNBOUND) {
+               u8 ssap = nfc_llcp_reserve_sdp_ssap(local);
+
+               pr_debug("First client, reserving %d\n", ssap);
+
+               if (ssap == LLCP_SAP_MAX) {
+                       reason = LLCP_DM_REJ;
+                       release_sock(&sock->sk);
+                       sock_put(&sock->sk);
+                       goto fail;
+               }
+
+               sock->ssap = ssap;
+       }
+
+       new_sk = nfc_llcp_sock_alloc(NULL, parent->sk_type, GFP_ATOMIC);
+       if (new_sk == NULL) {
+               reason = LLCP_DM_REJ;
+               release_sock(&sock->sk);
+               sock_put(&sock->sk);
+               goto fail;
+       }
+
+       new_sock = nfc_llcp_sock(new_sk);
+       new_sock->dev = local->dev;
+       new_sock->local = nfc_llcp_local_get(local);
+       new_sock->rw = sock->rw;
+       new_sock->miux = sock->miux;
+       new_sock->remote_miu = local->remote_miu;
+       new_sock->nfc_protocol = sock->nfc_protocol;
+       new_sock->dsap = ssap;
+       new_sock->target_idx = local->target_idx;
+       new_sock->parent = parent;
+       new_sock->ssap = sock->ssap;
+       if (sock->ssap < LLCP_LOCAL_NUM_SAP && sock->ssap >= LLCP_WKS_NUM_SAP) {
+               atomic_t *client_count;
+
+               pr_debug("reserved_ssap %d for %p\n", sock->ssap, new_sock);
+
+               client_count =
+                       &local->local_sdp_cnt[sock->ssap - LLCP_WKS_NUM_SAP];
+
+               atomic_inc(client_count);
+               new_sock->reserved_ssap = sock->ssap;
+       }
+
+       nfc_llcp_parse_connection_tlv(new_sock, &skb->data[LLCP_HEADER_SIZE],
+                                     skb->len - LLCP_HEADER_SIZE);
+
+       pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk);
+
+       nfc_llcp_sock_link(&local->sockets, new_sk);
+
+       nfc_llcp_accept_enqueue(&sock->sk, new_sk);
+
+       nfc_get_device(local->dev->idx);
+
+       new_sk->sk_state = LLCP_CONNECTED;
+
+       /* Wake the listening processes */
+       parent->sk_data_ready(parent, 0);
+
+       /* Send CC */
+       nfc_llcp_send_cc(new_sock);
+
+       release_sock(&sock->sk);
+       sock_put(&sock->sk);
+
+       return;
+
+fail:
+       /* Send DM */
+       nfc_llcp_send_dm(local, dsap, ssap, reason);
+}
+
+int nfc_llcp_queue_i_frames(struct nfc_llcp_sock *sock)
+{
+       int nr_frames = 0;
+       struct nfc_llcp_local *local = sock->local;
+
+       pr_debug("Remote ready %d tx queue len %d remote rw %d",
+                sock->remote_ready, skb_queue_len(&sock->tx_pending_queue),
+                sock->remote_rw);
+
+       /* Try to queue some I frames for transmission */
+       while (sock->remote_ready &&
+              skb_queue_len(&sock->tx_pending_queue) < sock->remote_rw) {
+               struct sk_buff *pdu;
+
+               pdu = skb_dequeue(&sock->tx_queue);
+               if (pdu == NULL)
+                       break;
+
+               /* Update N(S)/N(R) */
+               nfc_llcp_set_nrns(sock, pdu);
+
+               skb_queue_tail(&local->tx_queue, pdu);
+               nr_frames++;
+       }
+
+       return nr_frames;
+}
+
+static void nfc_llcp_recv_hdlc(struct nfc_llcp_local *local,
+                              struct sk_buff *skb)
+{
+       struct nfc_llcp_sock *llcp_sock;
+       struct sock *sk;
+       u8 dsap, ssap, ptype, ns, nr;
+
+       ptype = nfc_llcp_ptype(skb);
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+       ns = nfc_llcp_ns(skb);
+       nr = nfc_llcp_nr(skb);
+
+       pr_debug("%d %d R %d S %d\n", dsap, ssap, nr, ns);
+
+       llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+       if (llcp_sock == NULL) {
+               nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+               return;
+       }
+
+       sk = &llcp_sock->sk;
+       lock_sock(sk);
+       if (sk->sk_state == LLCP_CLOSED) {
+               release_sock(sk);
+               nfc_llcp_sock_put(llcp_sock);
+       }
+
+       /* Pass the payload upstream */
+       if (ptype == LLCP_PDU_I) {
+               pr_debug("I frame, queueing on %p\n", &llcp_sock->sk);
+
+               if (ns == llcp_sock->recv_n)
+                       llcp_sock->recv_n = (llcp_sock->recv_n + 1) % 16;
+               else
+                       pr_err("Received out of sequence I PDU\n");
+
+               skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE);
+               if (!sock_queue_rcv_skb(&llcp_sock->sk, skb)) {
+                       /*
+                        * I frames will be freed from the socket layer, so we
+                        * need to keep them alive until someone receives them.
+                        */
+                       skb_get(skb);
+               } else {
+                       pr_err("Receive queue is full\n");
+               }
+       }
+
+       /* Remove skbs from the pending queue */
+       if (llcp_sock->send_ack_n != nr) {
+               struct sk_buff *s, *tmp;
+               u8 n;
+
+               llcp_sock->send_ack_n = nr;
+
+               /* Remove and free all skbs until ns == nr */
+               skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) {
+                       n = nfc_llcp_ns(s);
+
+                       skb_unlink(s, &llcp_sock->tx_pending_queue);
+                       kfree_skb(s);
+
+                       if (n == nr)
+                               break;
+               }
+
+               /* Re-queue the remaining skbs for transmission */
+               skb_queue_reverse_walk_safe(&llcp_sock->tx_pending_queue,
+                                           s, tmp) {
+                       skb_unlink(s, &llcp_sock->tx_pending_queue);
+                       skb_queue_head(&local->tx_queue, s);
+               }
+       }
+
+       if (ptype == LLCP_PDU_RR)
+               llcp_sock->remote_ready = true;
+       else if (ptype == LLCP_PDU_RNR)
+               llcp_sock->remote_ready = false;
+
+       if (nfc_llcp_queue_i_frames(llcp_sock) == 0 && ptype == LLCP_PDU_I)
+               nfc_llcp_send_rr(llcp_sock);
+
+       release_sock(sk);
+       nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_disc(struct nfc_llcp_local *local,
+                              struct sk_buff *skb)
+{
+       struct nfc_llcp_sock *llcp_sock;
+       struct sock *sk;
+       u8 dsap, ssap;
+
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+
+       if ((dsap == 0) && (ssap == 0)) {
+               pr_debug("Connection termination");
+               nfc_dep_link_down(local->dev);
+               return;
+       }
+
+       llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+       if (llcp_sock == NULL) {
+               nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+               return;
+       }
+
+       sk = &llcp_sock->sk;
+       lock_sock(sk);
+
+       nfc_llcp_socket_purge(llcp_sock);
+
+       if (sk->sk_state == LLCP_CLOSED) {
+               release_sock(sk);
+               nfc_llcp_sock_put(llcp_sock);
+       }
+
+       if (sk->sk_state == LLCP_CONNECTED) {
+               nfc_put_device(local->dev);
+               sk->sk_state = LLCP_CLOSED;
+               sk->sk_state_change(sk);
+       }
+
+       nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_DISC);
+
+       release_sock(sk);
+       nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_cc(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+       struct nfc_llcp_sock *llcp_sock;
+       struct sock *sk;
+       u8 dsap, ssap;
+
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+
+       llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+       if (llcp_sock == NULL) {
+               pr_err("Invalid CC\n");
+               nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN);
+
+               return;
+       }
+
+       sk = &llcp_sock->sk;
+
+       /* Unlink from connecting and link to the client array */
+       nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+       nfc_llcp_sock_link(&local->sockets, sk);
+       llcp_sock->dsap = ssap;
+
+       nfc_llcp_parse_connection_tlv(llcp_sock, &skb->data[LLCP_HEADER_SIZE],
+                                     skb->len - LLCP_HEADER_SIZE);
+
+       sk->sk_state = LLCP_CONNECTED;
+       sk->sk_state_change(sk);
+
+       nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_dm(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+       struct nfc_llcp_sock *llcp_sock;
+       struct sock *sk;
+       u8 dsap, ssap, reason;
+
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+       reason = skb->data[2];
+
+       pr_debug("%d %d reason %d\n", ssap, dsap, reason);
+
+       switch (reason) {
+       case LLCP_DM_NOBOUND:
+       case LLCP_DM_REJ:
+               llcp_sock = nfc_llcp_connecting_sock_get(local, dsap);
+               break;
+
+       default:
+               llcp_sock = nfc_llcp_sock_get(local, dsap, ssap);
+               break;
+       }
+
+       if (llcp_sock == NULL) {
+               pr_debug("Already closed\n");
+               return;
+       }
+
+       sk = &llcp_sock->sk;
+
+       sk->sk_err = ENXIO;
+       sk->sk_state = LLCP_CLOSED;
+       sk->sk_state_change(sk);
+
+       nfc_llcp_sock_put(llcp_sock);
+}
+
+static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
+                             struct sk_buff *skb)
+{
+       struct nfc_llcp_sock *llcp_sock;
+       u8 dsap, ssap, *tlv, type, length, tid, sap;
+       u16 tlv_len, offset;
+       char *service_name;
+       size_t service_name_len;
+       struct nfc_llcp_sdp_tlv *sdp;
+       HLIST_HEAD(llc_sdres_list);
+       size_t sdres_tlvs_len;
+       HLIST_HEAD(nl_sdres_list);
+
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+
+       pr_debug("%d %d\n", dsap, ssap);
+
+       if (dsap != LLCP_SAP_SDP || ssap != LLCP_SAP_SDP) {
+               pr_err("Wrong SNL SAP\n");
+               return;
+       }
+
+       tlv = &skb->data[LLCP_HEADER_SIZE];
+       tlv_len = skb->len - LLCP_HEADER_SIZE;
+       offset = 0;
+       sdres_tlvs_len = 0;
+
+       while (offset < tlv_len) {
+               type = tlv[0];
+               length = tlv[1];
+
+               switch (type) {
+               case LLCP_TLV_SDREQ:
+                       tid = tlv[2];
+                       service_name = (char *) &tlv[3];
+                       service_name_len = length - 1;
+
+                       pr_debug("Looking for %.16s\n", service_name);
+
+                       if (service_name_len == strlen("urn:nfc:sn:sdp") &&
+                           !strncmp(service_name, "urn:nfc:sn:sdp",
+                                    service_name_len)) {
+                               sap = 1;
+                               goto add_snl;
+                       }
+
+                       llcp_sock = nfc_llcp_sock_from_sn(local, service_name,
+                                                         service_name_len);
+                       if (!llcp_sock) {
+                               sap = 0;
+                               goto add_snl;
+                       }
+
+                       /*
+                        * We found a socket but its ssap has not been reserved
+                        * yet. We need to assign it for good and send a reply.
+                        * The ssap will be freed when the socket is closed.
+                        */
+                       if (llcp_sock->ssap == LLCP_SDP_UNBOUND) {
+                               atomic_t *client_count;
+
+                               sap = nfc_llcp_reserve_sdp_ssap(local);
+
+                               pr_debug("Reserving %d\n", sap);
+
+                               if (sap == LLCP_SAP_MAX) {
+                                       sap = 0;
+                                       goto add_snl;
+                               }
+
+                               client_count =
+                                       &local->local_sdp_cnt[sap -
+                                                             LLCP_WKS_NUM_SAP];
+
+                               atomic_inc(client_count);
+
+                               llcp_sock->ssap = sap;
+                               llcp_sock->reserved_ssap = sap;
+                       } else {
+                               sap = llcp_sock->ssap;
+                       }
+
+                       pr_debug("%p %d\n", llcp_sock, sap);
+
+add_snl:
+                       sdp = nfc_llcp_build_sdres_tlv(tid, sap);
+                       if (sdp == NULL)
+                               goto exit;
+
+                       sdres_tlvs_len += sdp->tlv_len;
+                       hlist_add_head(&sdp->node, &llc_sdres_list);
+                       break;
+
+               case LLCP_TLV_SDRES:
+                       mutex_lock(&local->sdreq_lock);
+
+                       pr_debug("LLCP_TLV_SDRES: searching tid %d\n", tlv[2]);
+
+                       hlist_for_each_entry(sdp, &local->pending_sdreqs, node) {
+                               if (sdp->tid != tlv[2])
+                                       continue;
+
+                               sdp->sap = tlv[3];
+
+                               pr_debug("Found: uri=%s, sap=%d\n",
+                                        sdp->uri, sdp->sap);
+
+                               hlist_del(&sdp->node);
+
+                               hlist_add_head(&sdp->node, &nl_sdres_list);
+
+                               break;
+                       }
+
+                       mutex_unlock(&local->sdreq_lock);
+                       break;
+
+               default:
+                       pr_err("Invalid SNL tlv value 0x%x\n", type);
+                       break;
+               }
+
+               offset += length + 2;
+               tlv += length + 2;
+       }
+
+exit:
+       if (!hlist_empty(&nl_sdres_list))
+               nfc_genl_llc_send_sdres(local->dev, &nl_sdres_list);
+
+       if (!hlist_empty(&llc_sdres_list))
+               nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
+}
+
+static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+       u8 ptype;
+       u16 pdu_len;
+       struct sk_buff *new_skb;
+
+       if (skb->len <= LLCP_HEADER_SIZE) {
+               pr_err("Malformed AGF PDU\n");
+               return;
+       }
+
+       skb_pull(skb, LLCP_HEADER_SIZE);
+
+       while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
+               pdu_len = skb->data[0] << 8 | skb->data[1];
+
+               skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
+
+               if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
+                       pr_err("Malformed AGF PDU\n");
+                       return;
+               }
+
+               ptype = nfc_llcp_ptype(skb);
+
+               if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
+                       goto next;
+
+               new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
+               if (new_skb == NULL) {
+                       pr_err("Could not allocate PDU\n");
+                       return;
+               }
+
+               memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
+
+               nfc_llcp_rx_skb(local, new_skb);
+
+               kfree_skb(new_skb);
+next:
+               skb_pull(skb, pdu_len);
+       }
+}
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+       u8 dsap, ssap, ptype;
+
+       ptype = nfc_llcp_ptype(skb);
+       dsap = nfc_llcp_dsap(skb);
+       ssap = nfc_llcp_ssap(skb);
+
+       pr_debug("ptype 0x%x dsap 0x%x ssap 0x%x\n", ptype, dsap, ssap);
+
+       if (ptype != LLCP_PDU_SYMM)
+               print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
+                              16, 1, skb->data, skb->len, true);
+
+       switch (ptype) {
+       case LLCP_PDU_SYMM:
+               pr_debug("SYMM\n");
+               break;
+
+       case LLCP_PDU_UI:
+               pr_debug("UI\n");
+               nfc_llcp_recv_ui(local, skb);
+               break;
+
+       case LLCP_PDU_CONNECT:
+               pr_debug("CONNECT\n");
+               nfc_llcp_recv_connect(local, skb);
+               break;
+
+       case LLCP_PDU_DISC:
+               pr_debug("DISC\n");
+               nfc_llcp_recv_disc(local, skb);
+               break;
+
+       case LLCP_PDU_CC:
+               pr_debug("CC\n");
+               nfc_llcp_recv_cc(local, skb);
+               break;
+
+       case LLCP_PDU_DM:
+               pr_debug("DM\n");
+               nfc_llcp_recv_dm(local, skb);
+               break;
+
+       case LLCP_PDU_SNL:
+               pr_debug("SNL\n");
+               nfc_llcp_recv_snl(local, skb);
+               break;
+
+       case LLCP_PDU_I:
+       case LLCP_PDU_RR:
+       case LLCP_PDU_RNR:
+               pr_debug("I frame\n");
+               nfc_llcp_recv_hdlc(local, skb);
+               break;
+
+       case LLCP_PDU_AGF:
+               pr_debug("AGF frame\n");
+               nfc_llcp_recv_agf(local, skb);
+               break;
+       }
+}
+
+static void nfc_llcp_rx_work(struct work_struct *work)
+{
+       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                   rx_work);
+       struct sk_buff *skb;
+
+       skb = local->rx_pending;
+       if (skb == NULL) {
+               pr_debug("No pending SKB\n");
+               return;
+       }
+
+       __net_timestamp(skb);
+
+       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+
+       nfc_llcp_rx_skb(local, skb);
+
+       schedule_work(&local->tx_work);
+       kfree_skb(local->rx_pending);
+       local->rx_pending = NULL;
+}
+
+static void __nfc_llcp_recv(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+       local->rx_pending = skb;
+       del_timer(&local->link_timer);
+       schedule_work(&local->rx_work);
+}
+
+void nfc_llcp_recv(void *data, struct sk_buff *skb, int err)
+{
+       struct nfc_llcp_local *local = (struct nfc_llcp_local *) data;
+
+       pr_debug("Received an LLCP PDU\n");
+       if (err < 0) {
+               pr_err("err %d\n", err);
+               return;
+       }
+
+       __nfc_llcp_recv(local, skb);
+}
+
+int nfc_llcp_data_received(struct nfc_dev *dev, struct sk_buff *skb)
+{
+       struct nfc_llcp_local *local;
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL)
+               return -ENODEV;
+
+       __nfc_llcp_recv(local, skb);
+
+       return 0;
+}
+
+void nfc_llcp_mac_is_down(struct nfc_dev *dev)
+{
+       struct nfc_llcp_local *local;
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL)
+               return;
+
+       local->remote_miu = LLCP_DEFAULT_MIU;
+       local->remote_lto = LLCP_DEFAULT_LTO;
+
+       /* Close and purge all existing sockets */
+       nfc_llcp_socket_release(local, true, 0);
+}
+
+void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
+                       u8 comm_mode, u8 rf_mode)
+{
+       struct nfc_llcp_local *local;
+
+       pr_debug("rf mode %d\n", rf_mode);
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL)
+               return;
+
+       local->target_idx = target_idx;
+       local->comm_mode = comm_mode;
+       local->rf_mode = rf_mode;
+
+       if (rf_mode == NFC_RF_INITIATOR) {
+               pr_debug("Queueing Tx work\n");
+
+               schedule_work(&local->tx_work);
+       } else {
+               mod_timer(&local->link_timer,
+                         jiffies + msecs_to_jiffies(local->remote_lto));
+       }
+}
+
+int nfc_llcp_register_device(struct nfc_dev *ndev)
+{
+       struct nfc_llcp_local *local;
+
+       local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL);
+       if (local == NULL)
+               return -ENOMEM;
+
+       local->dev = ndev;
+       INIT_LIST_HEAD(&local->list);
+       kref_init(&local->ref);
+       mutex_init(&local->sdp_lock);
+       init_timer(&local->link_timer);
+       local->link_timer.data = (unsigned long) local;
+       local->link_timer.function = nfc_llcp_symm_timer;
+
+       skb_queue_head_init(&local->tx_queue);
+       INIT_WORK(&local->tx_work, nfc_llcp_tx_work);
+
+       local->rx_pending = NULL;
+       INIT_WORK(&local->rx_work, nfc_llcp_rx_work);
+
+       INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work);
+
+       rwlock_init(&local->sockets.lock);
+       rwlock_init(&local->connecting_sockets.lock);
+       rwlock_init(&local->raw_sockets.lock);
+
+       local->lto = 150; /* 1500 ms */
+       local->rw = LLCP_MAX_RW;
+       local->miux = cpu_to_be16(LLCP_MAX_MIUX);
+
+       nfc_llcp_build_gb(local);
+
+       local->remote_miu = LLCP_DEFAULT_MIU;
+       local->remote_lto = LLCP_DEFAULT_LTO;
+
+       mutex_init(&local->sdreq_lock);
+       INIT_HLIST_HEAD(&local->pending_sdreqs);
+       init_timer(&local->sdreq_timer);
+       local->sdreq_timer.data = (unsigned long) local;
+       local->sdreq_timer.function = nfc_llcp_sdreq_timer;
+       INIT_WORK(&local->sdreq_timeout_work, nfc_llcp_sdreq_timeout_work);
+
+       list_add(&local->list, &llcp_devices);
+
+       return 0;
+}
+
+void nfc_llcp_unregister_device(struct nfc_dev *dev)
+{
+       struct nfc_llcp_local *local = nfc_llcp_find_local(dev);
+
+       if (local == NULL) {
+               pr_debug("No such device\n");
+               return;
+       }
+
+       local_cleanup(local);
+
+       nfc_llcp_local_put(local);
+}
+
+int __init nfc_llcp_init(void)
+{
+       INIT_LIST_HEAD(&llcp_devices);
+
+       return nfc_llcp_sock_init();
+}
+
+void nfc_llcp_exit(void)
+{
+       nfc_llcp_sock_exit();
+}
diff --git a/net/nfc/llcp_sock.c b/net/nfc/llcp_sock.c
new file mode 100644 (file)
index 0000000..38f08c3
--- /dev/null
@@ -0,0 +1,1029 @@
+/*
+ * Copyright (C) 2011  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 as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the
+ * Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+
+#define pr_fmt(fmt) "llcp: %s: " fmt, __func__
+
+#include <linux/init.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/nfc.h>
+
+#include "nfc.h"
+#include "llcp.h"
+
+static int sock_wait_state(struct sock *sk, int state, unsigned long timeo)
+{
+       DECLARE_WAITQUEUE(wait, current);
+       int err = 0;
+
+       pr_debug("sk %p", sk);
+
+       add_wait_queue(sk_sleep(sk), &wait);
+       set_current_state(TASK_INTERRUPTIBLE);
+
+       while (sk->sk_state != state) {
+               if (!timeo) {
+                       err = -EINPROGRESS;
+                       break;
+               }
+
+               if (signal_pending(current)) {
+                       err = sock_intr_errno(timeo);
+                       break;
+               }
+
+               release_sock(sk);
+               timeo = schedule_timeout(timeo);
+               lock_sock(sk);
+               set_current_state(TASK_INTERRUPTIBLE);
+
+               err = sock_error(sk);
+               if (err)
+                       break;
+       }
+
+       __set_current_state(TASK_RUNNING);
+       remove_wait_queue(sk_sleep(sk), &wait);
+       return err;
+}
+
+static struct proto llcp_sock_proto = {
+       .name     = "NFC_LLCP",
+       .owner    = THIS_MODULE,
+       .obj_size = sizeof(struct nfc_llcp_sock),
+};
+
+static int llcp_sock_bind(struct socket *sock, struct sockaddr *addr, int alen)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       struct nfc_llcp_local *local;
+       struct nfc_dev *dev;
+       struct sockaddr_nfc_llcp llcp_addr;
+       int len, ret = 0;
+
+       if (!addr || addr->sa_family != AF_NFC)
+               return -EINVAL;
+
+       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+       memset(&llcp_addr, 0, sizeof(llcp_addr));
+       len = min_t(unsigned int, sizeof(llcp_addr), alen);
+       memcpy(&llcp_addr, addr, len);
+
+       /* This is going to be a listening socket, dsap must be 0 */
+       if (llcp_addr.dsap != 0)
+               return -EINVAL;
+
+       lock_sock(sk);
+
+       if (sk->sk_state != LLCP_CLOSED) {
+               ret = -EBADFD;
+               goto error;
+       }
+
+       dev = nfc_get_device(llcp_addr.dev_idx);
+       if (dev == NULL) {
+               ret = -ENODEV;
+               goto error;
+       }
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL) {
+               ret = -ENODEV;
+               goto put_dev;
+       }
+
+       llcp_sock->dev = dev;
+       llcp_sock->local = nfc_llcp_local_get(local);
+       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+       llcp_sock->service_name_len = min_t(unsigned int,
+                                           llcp_addr.service_name_len,
+                                           NFC_LLCP_MAX_SERVICE_NAME);
+       llcp_sock->service_name = kmemdup(llcp_addr.service_name,
+                                         llcp_sock->service_name_len,
+                                         GFP_KERNEL);
+
+       llcp_sock->ssap = nfc_llcp_get_sdp_ssap(local, llcp_sock);
+       if (llcp_sock->ssap == LLCP_SAP_MAX) {
+               ret = -EADDRINUSE;
+               goto put_dev;
+       }
+
+       llcp_sock->reserved_ssap = llcp_sock->ssap;
+
+       nfc_llcp_sock_link(&local->sockets, sk);
+
+       pr_debug("Socket bound to SAP %d\n", llcp_sock->ssap);
+
+       sk->sk_state = LLCP_BOUND;
+
+put_dev:
+       nfc_put_device(dev);
+
+error:
+       release_sock(sk);
+       return ret;
+}
+
+static int llcp_raw_sock_bind(struct socket *sock, struct sockaddr *addr,
+                             int alen)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       struct nfc_llcp_local *local;
+       struct nfc_dev *dev;
+       struct sockaddr_nfc_llcp llcp_addr;
+       int len, ret = 0;
+
+       if (!addr || addr->sa_family != AF_NFC)
+               return -EINVAL;
+
+       pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family);
+
+       memset(&llcp_addr, 0, sizeof(llcp_addr));
+       len = min_t(unsigned int, sizeof(llcp_addr), alen);
+       memcpy(&llcp_addr, addr, len);
+
+       lock_sock(sk);
+
+       if (sk->sk_state != LLCP_CLOSED) {
+               ret = -EBADFD;
+               goto error;
+       }
+
+       dev = nfc_get_device(llcp_addr.dev_idx);
+       if (dev == NULL) {
+               ret = -ENODEV;
+               goto error;
+       }
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL) {
+               ret = -ENODEV;
+               goto put_dev;
+       }
+
+       llcp_sock->dev = dev;
+       llcp_sock->local = nfc_llcp_local_get(local);
+       llcp_sock->nfc_protocol = llcp_addr.nfc_protocol;
+
+       nfc_llcp_sock_link(&local->raw_sockets, sk);
+
+       sk->sk_state = LLCP_BOUND;
+
+put_dev:
+       nfc_put_device(dev);
+
+error:
+       release_sock(sk);
+       return ret;
+}
+
+static int llcp_sock_listen(struct socket *sock, int backlog)
+{
+       struct sock *sk = sock->sk;
+       int ret = 0;
+
+       pr_debug("sk %p backlog %d\n", sk, backlog);
+
+       lock_sock(sk);
+
+       if ((sock->type != SOCK_SEQPACKET && sock->type != SOCK_STREAM) ||
+           sk->sk_state != LLCP_BOUND) {
+               ret = -EBADFD;
+               goto error;
+       }
+
+       sk->sk_max_ack_backlog = backlog;
+       sk->sk_ack_backlog = 0;
+
+       pr_debug("Socket listening\n");
+       sk->sk_state = LLCP_LISTEN;
+
+error:
+       release_sock(sk);
+
+       return ret;
+}
+
+static int nfc_llcp_setsockopt(struct socket *sock, int level, int optname,
+                              char __user *optval, unsigned int optlen)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       u32 opt;
+       int err = 0;
+
+       pr_debug("%p optname %d\n", sk, optname);
+
+       if (level != SOL_NFC)
+               return -ENOPROTOOPT;
+
+       lock_sock(sk);
+
+       switch (optname) {
+       case NFC_LLCP_RW:
+               if (sk->sk_state == LLCP_CONNECTED ||
+                   sk->sk_state == LLCP_BOUND ||
+                   sk->sk_state == LLCP_LISTEN) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               if (get_user(opt, (u32 __user *) optval)) {
+                       err = -EFAULT;
+                       break;
+               }
+
+               if (opt > LLCP_MAX_RW) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               llcp_sock->rw = (u8) opt;
+
+               break;
+
+       case NFC_LLCP_MIUX:
+               if (sk->sk_state == LLCP_CONNECTED ||
+                   sk->sk_state == LLCP_BOUND ||
+                   sk->sk_state == LLCP_LISTEN) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               if (get_user(opt, (u32 __user *) optval)) {
+                       err = -EFAULT;
+                       break;
+               }
+
+               if (opt > LLCP_MAX_MIUX) {
+                       err = -EINVAL;
+                       break;
+               }
+
+               llcp_sock->miux = cpu_to_be16((u16) opt);
+
+               break;
+
+       default:
+               err = -ENOPROTOOPT;
+               break;
+       }
+
+       release_sock(sk);
+
+       pr_debug("%p rw %d miux %d\n", llcp_sock,
+                llcp_sock->rw, llcp_sock->miux);
+
+       return err;
+}
+
+static int nfc_llcp_getsockopt(struct socket *sock, int level, int optname,
+                              char __user *optval, int __user *optlen)
+{
+       struct nfc_llcp_local *local;
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       int len, err = 0;
+       u16 miux, remote_miu;
+       u8 rw;
+
+       pr_debug("%p optname %d\n", sk, optname);
+
+       if (level != SOL_NFC)
+               return -ENOPROTOOPT;
+
+       if (get_user(len, optlen))
+               return -EFAULT;
+
+       local = llcp_sock->local;
+       if (!local)
+               return -ENODEV;
+
+       len = min_t(u32, len, sizeof(u32));
+
+       lock_sock(sk);
+
+       switch (optname) {
+       case NFC_LLCP_RW:
+               rw = llcp_sock->rw > LLCP_MAX_RW ? local->rw : llcp_sock->rw;
+               if (put_user(rw, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_MIUX:
+               miux = be16_to_cpu(llcp_sock->miux) > LLCP_MAX_MIUX ?
+                       be16_to_cpu(local->miux) : be16_to_cpu(llcp_sock->miux);
+
+               if (put_user(miux, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_REMOTE_MIU:
+               remote_miu = llcp_sock->remote_miu > LLCP_MAX_MIU ?
+                               local->remote_miu : llcp_sock->remote_miu;
+
+               if (put_user(remote_miu, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_REMOTE_LTO:
+               if (put_user(local->remote_lto / 10, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       case NFC_LLCP_REMOTE_RW:
+               if (put_user(llcp_sock->remote_rw, (u32 __user *) optval))
+                       err = -EFAULT;
+
+               break;
+
+       default:
+               err = -ENOPROTOOPT;
+               break;
+       }
+
+       release_sock(sk);
+
+       if (put_user(len, optlen))
+               return -EFAULT;
+
+       return err;
+}
+
+void nfc_llcp_accept_unlink(struct sock *sk)
+{
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+       pr_debug("state %d\n", sk->sk_state);
+
+       list_del_init(&llcp_sock->accept_queue);
+       sk_acceptq_removed(llcp_sock->parent);
+       llcp_sock->parent = NULL;
+
+       sock_put(sk);
+}
+
+void nfc_llcp_accept_enqueue(struct sock *parent, struct sock *sk)
+{
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       struct nfc_llcp_sock *llcp_sock_parent = nfc_llcp_sock(parent);
+
+       /* Lock will be free from unlink */
+       sock_hold(sk);
+
+       list_add_tail(&llcp_sock->accept_queue,
+                     &llcp_sock_parent->accept_queue);
+       llcp_sock->parent = parent;
+       sk_acceptq_added(parent);
+}
+
+struct sock *nfc_llcp_accept_dequeue(struct sock *parent,
+                                    struct socket *newsock)
+{
+       struct nfc_llcp_sock *lsk, *n, *llcp_parent;
+       struct sock *sk;
+
+       llcp_parent = nfc_llcp_sock(parent);
+
+       list_for_each_entry_safe(lsk, n, &llcp_parent->accept_queue,
+                                accept_queue) {
+               sk = &lsk->sk;
+               lock_sock(sk);
+
+               if (sk->sk_state == LLCP_CLOSED) {
+                       release_sock(sk);
+                       nfc_llcp_accept_unlink(sk);
+                       continue;
+               }
+
+               if (sk->sk_state == LLCP_CONNECTED || !newsock) {
+                       list_del_init(&lsk->accept_queue);
+                       sock_put(sk);
+
+                       if (newsock)
+                               sock_graft(sk, newsock);
+
+                       release_sock(sk);
+
+                       pr_debug("Returning sk state %d\n", sk->sk_state);
+
+                       sk_acceptq_removed(parent);
+
+                       return sk;
+               }
+
+               release_sock(sk);
+       }
+
+       return NULL;
+}
+
+static int llcp_sock_accept(struct socket *sock, struct socket *newsock,
+                           int flags)
+{
+       DECLARE_WAITQUEUE(wait, current);
+       struct sock *sk = sock->sk, *new_sk;
+       long timeo;
+       int ret = 0;
+
+       pr_debug("parent %p\n", sk);
+
+       lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
+
+       if (sk->sk_state != LLCP_LISTEN) {
+               ret = -EBADFD;
+               goto error;
+       }
+
+       timeo = sock_rcvtimeo(sk, flags & O_NONBLOCK);
+
+       /* Wait for an incoming connection. */
+       add_wait_queue_exclusive(sk_sleep(sk), &wait);
+       while (!(new_sk = nfc_llcp_accept_dequeue(sk, newsock))) {
+               set_current_state(TASK_INTERRUPTIBLE);
+
+               if (!timeo) {
+                       ret = -EAGAIN;
+                       break;
+               }
+
+               if (signal_pending(current)) {
+                       ret = sock_intr_errno(timeo);
+                       break;
+               }
+
+               release_sock(sk);
+               timeo = schedule_timeout(timeo);
+               lock_sock_nested(sk, SINGLE_DEPTH_NESTING);
+       }
+       __set_current_state(TASK_RUNNING);
+       remove_wait_queue(sk_sleep(sk), &wait);
+
+       if (ret)
+               goto error;
+
+       newsock->state = SS_CONNECTED;
+
+       pr_debug("new socket %p\n", new_sk);
+
+error:
+       release_sock(sk);
+
+       return ret;
+}
+
+static int llcp_sock_getname(struct socket *sock, struct sockaddr *uaddr,
+                            int *len, int peer)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       DECLARE_SOCKADDR(struct sockaddr_nfc_llcp *, llcp_addr, uaddr);
+
+       if (llcp_sock == NULL || llcp_sock->dev == NULL)
+               return -EBADFD;
+
+       pr_debug("%p %d %d %d\n", sk, llcp_sock->target_idx,
+                llcp_sock->dsap, llcp_sock->ssap);
+
+       uaddr->sa_family = AF_NFC;
+
+       *len = sizeof(struct sockaddr_nfc_llcp);
+
+       llcp_addr->dev_idx = llcp_sock->dev->idx;
+       llcp_addr->target_idx = llcp_sock->target_idx;
+       llcp_addr->dsap = llcp_sock->dsap;
+       llcp_addr->ssap = llcp_sock->ssap;
+       llcp_addr->service_name_len = llcp_sock->service_name_len;
+       memcpy(llcp_addr->service_name, llcp_sock->service_name,
+              llcp_addr->service_name_len);
+
+       return 0;
+}
+
+static inline unsigned int llcp_accept_poll(struct sock *parent)
+{
+       struct nfc_llcp_sock *llcp_sock, *n, *parent_sock;
+       struct sock *sk;
+
+       parent_sock = nfc_llcp_sock(parent);
+
+       list_for_each_entry_safe(llcp_sock, n, &parent_sock->accept_queue,
+                                accept_queue) {
+               sk = &llcp_sock->sk;
+
+               if (sk->sk_state == LLCP_CONNECTED)
+                       return POLLIN | POLLRDNORM;
+       }
+
+       return 0;
+}
+
+static unsigned int llcp_sock_poll(struct file *file, struct socket *sock,
+                                  poll_table *wait)
+{
+       struct sock *sk = sock->sk;
+       unsigned int mask = 0;
+
+       pr_debug("%p\n", sk);
+
+       sock_poll_wait(file, sk_sleep(sk), wait);
+
+       if (sk->sk_state == LLCP_LISTEN)
+               return llcp_accept_poll(sk);
+
+       if (sk->sk_err || !skb_queue_empty(&sk->sk_error_queue))
+               mask |= POLLERR |
+                       (sock_flag(sk, SOCK_SELECT_ERR_QUEUE) ? POLLPRI : 0);
+
+       if (!skb_queue_empty(&sk->sk_receive_queue))
+               mask |= POLLIN | POLLRDNORM;
+
+       if (sk->sk_state == LLCP_CLOSED)
+               mask |= POLLHUP;
+
+       if (sk->sk_shutdown & RCV_SHUTDOWN)
+               mask |= POLLRDHUP | POLLIN | POLLRDNORM;
+
+       if (sk->sk_shutdown == SHUTDOWN_MASK)
+               mask |= POLLHUP;
+
+       if (sock_writeable(sk))
+               mask |= POLLOUT | POLLWRNORM | POLLWRBAND;
+       else
+               set_bit(SOCK_ASYNC_NOSPACE, &sk->sk_socket->flags);
+
+       pr_debug("mask 0x%x\n", mask);
+
+       return mask;
+}
+
+static int llcp_sock_release(struct socket *sock)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_local *local;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       int err = 0;
+
+       if (!sk)
+               return 0;
+
+       pr_debug("%p\n", sk);
+
+       local = llcp_sock->local;
+       if (local == NULL) {
+               err = -ENODEV;
+               goto out;
+       }
+
+       lock_sock(sk);
+
+       /* Send a DISC */
+       if (sk->sk_state == LLCP_CONNECTED)
+               nfc_llcp_disconnect(llcp_sock);
+
+       if (sk->sk_state == LLCP_LISTEN) {
+               struct nfc_llcp_sock *lsk, *n;
+               struct sock *accept_sk;
+
+               list_for_each_entry_safe(lsk, n, &llcp_sock->accept_queue,
+                                        accept_queue) {
+                       accept_sk = &lsk->sk;
+                       lock_sock(accept_sk);
+
+                       nfc_llcp_disconnect(lsk);
+                       nfc_llcp_accept_unlink(accept_sk);
+
+                       release_sock(accept_sk);
+               }
+       }
+
+       if (llcp_sock->reserved_ssap < LLCP_SAP_MAX)
+               nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap);
+
+       release_sock(sk);
+
+       if (sock->type == SOCK_RAW)
+               nfc_llcp_sock_unlink(&local->raw_sockets, sk);
+       else
+               nfc_llcp_sock_unlink(&local->sockets, sk);
+
+out:
+       sock_orphan(sk);
+       sock_put(sk);
+
+       return err;
+}
+
+static int llcp_sock_connect(struct socket *sock, struct sockaddr *_addr,
+                            int len, int flags)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       struct sockaddr_nfc_llcp *addr = (struct sockaddr_nfc_llcp *)_addr;
+       struct nfc_dev *dev;
+       struct nfc_llcp_local *local;
+       int ret = 0;
+
+       pr_debug("sock %p sk %p flags 0x%x\n", sock, sk, flags);
+
+       if (!addr || len < sizeof(struct sockaddr_nfc) ||
+           addr->sa_family != AF_NFC)
+               return -EINVAL;
+
+       if (addr->service_name_len == 0 && addr->dsap == 0)
+               return -EINVAL;
+
+       pr_debug("addr dev_idx=%u target_idx=%u protocol=%u\n", addr->dev_idx,
+                addr->target_idx, addr->nfc_protocol);
+
+       lock_sock(sk);
+
+       if (sk->sk_state == LLCP_CONNECTED) {
+               ret = -EISCONN;
+               goto error;
+       }
+
+       dev = nfc_get_device(addr->dev_idx);
+       if (dev == NULL) {
+               ret = -ENODEV;
+               goto error;
+       }
+
+       local = nfc_llcp_find_local(dev);
+       if (local == NULL) {
+               ret = -ENODEV;
+               goto put_dev;
+       }
+
+       device_lock(&dev->dev);
+       if (dev->dep_link_up == false) {
+               ret = -ENOLINK;
+               device_unlock(&dev->dev);
+               goto put_dev;
+       }
+       device_unlock(&dev->dev);
+
+       if (local->rf_mode == NFC_RF_INITIATOR &&
+           addr->target_idx != local->target_idx) {
+               ret = -ENOLINK;
+               goto put_dev;
+       }
+
+       llcp_sock->dev = dev;
+       llcp_sock->local = nfc_llcp_local_get(local);
+       llcp_sock->remote_miu = llcp_sock->local->remote_miu;
+       llcp_sock->ssap = nfc_llcp_get_local_ssap(local);
+       if (llcp_sock->ssap == LLCP_SAP_MAX) {
+               ret = -ENOMEM;
+               goto put_dev;
+       }
+
+       llcp_sock->reserved_ssap = llcp_sock->ssap;
+
+       if (addr->service_name_len == 0)
+               llcp_sock->dsap = addr->dsap;
+       else
+               llcp_sock->dsap = LLCP_SAP_SDP;
+       llcp_sock->nfc_protocol = addr->nfc_protocol;
+       llcp_sock->service_name_len = min_t(unsigned int,
+                                           addr->service_name_len,
+                                           NFC_LLCP_MAX_SERVICE_NAME);
+       llcp_sock->service_name = kmemdup(addr->service_name,
+                                         llcp_sock->service_name_len,
+                                         GFP_KERNEL);
+
+       nfc_llcp_sock_link(&local->connecting_sockets, sk);
+
+       ret = nfc_llcp_send_connect(llcp_sock);
+       if (ret)
+               goto sock_unlink;
+
+       ret = sock_wait_state(sk, LLCP_CONNECTED,
+                             sock_sndtimeo(sk, flags & O_NONBLOCK));
+       if (ret)
+               goto sock_unlink;
+
+       release_sock(sk);
+
+       return 0;
+
+sock_unlink:
+       nfc_llcp_put_ssap(local, llcp_sock->ssap);
+
+       nfc_llcp_sock_unlink(&local->connecting_sockets, sk);
+
+put_dev:
+       nfc_put_device(dev);
+
+error:
+       release_sock(sk);
+       return ret;
+}
+
+static int llcp_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
+                            struct msghdr *msg, size_t len)
+{
+       struct sock *sk = sock->sk;
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+       int ret;
+
+       pr_debug("sock %p sk %p", sock, sk);
+
+       ret = sock_error(sk);
+       if (ret)
+               return ret;
+
+       if (msg->msg_flags & MSG_OOB)
+               return -EOPNOTSUPP;
+
+       lock_sock(sk);
+
+       if (sk->sk_type == SOCK_DGRAM) {
+               struct sockaddr_nfc_llcp *addr =
+                       (struct sockaddr_nfc_llcp *)msg->msg_name;
+
+               if (msg->msg_namelen < sizeof(*addr)) {
+                       release_sock(sk);
+                       return -EINVAL;
+               }
+
+               release_sock(sk);
+
+               return nfc_llcp_send_ui_frame(llcp_sock, addr->dsap, addr->ssap,
+                                             msg, len);
+       }
+
+       if (sk->sk_state != LLCP_CONNECTED) {
+               release_sock(sk);
+               return -ENOTCONN;
+       }
+
+       release_sock(sk);
+
+       return nfc_llcp_send_i_frame(llcp_sock, msg, len);
+}
+
+static int llcp_sock_recvmsg(struct kiocb *iocb, struct socket *sock,
+                            struct msghdr *msg, size_t len, int flags)
+{
+       int noblock = flags & MSG_DONTWAIT;
+       struct sock *sk = sock->sk;
+       unsigned int copied, rlen;
+       struct sk_buff *skb, *cskb;
+       int err = 0;
+
+       pr_debug("%p %zu\n", sk, len);
+
+       msg->msg_namelen = 0;
+
+       lock_sock(sk);
+
+       if (sk->sk_state == LLCP_CLOSED &&
+           skb_queue_empty(&sk->sk_receive_queue)) {
+               release_sock(sk);
+               return 0;
+       }
+
+       release_sock(sk);
+
+       if (flags & (MSG_OOB))
+               return -EOPNOTSUPP;
+
+       skb = skb_recv_datagram(sk, flags, noblock, &err);
+       if (!skb) {
+               pr_err("Recv datagram failed state %d %d %d",
+                      sk->sk_state, err, sock_error(sk));
+
+               if (sk->sk_shutdown & RCV_SHUTDOWN)
+                       return 0;
+
+               return err;
+       }
+
+       rlen = skb->len;                /* real length of skb */
+       copied = min_t(unsigned int, rlen, len);
+
+       cskb = skb;
+       if (skb_copy_datagram_iovec(cskb, 0, msg->msg_iov, copied)) {
+               if (!(flags & MSG_PEEK))
+                       skb_queue_head(&sk->sk_receive_queue, skb);
+               return -EFAULT;
+       }
+
+       sock_recv_timestamp(msg, sk, skb);
+
+       if (sk->sk_type == SOCK_DGRAM && msg->msg_name) {
+               struct nfc_llcp_ui_cb *ui_cb = nfc_llcp_ui_skb_cb(skb);
+               struct sockaddr_nfc_llcp *sockaddr =
+                       (struct sockaddr_nfc_llcp *) msg->msg_name;
+
+               msg->msg_namelen = sizeof(struct sockaddr_nfc_llcp);
+
+               pr_debug("Datagram socket %d %d\n", ui_cb->dsap, ui_cb->ssap);
+
+               memset(sockaddr, 0, sizeof(*sockaddr));
+               sockaddr->sa_family = AF_NFC;
+               sockaddr->nfc_protocol = NFC_PROTO_NFC_DEP;
+               sockaddr->dsap = ui_cb->dsap;
+               sockaddr->ssap = ui_cb->ssap;
+       }
+
+       /* Mark read part of skb as used */
+       if (!(flags & MSG_PEEK)) {
+
+               /* SOCK_STREAM: re-queue skb if it contains unreceived data */
+               if (sk->sk_type == SOCK_STREAM ||
+                   sk->sk_type == SOCK_DGRAM ||
+                   sk->sk_type == SOCK_RAW) {
+                       skb_pull(skb, copied);
+                       if (skb->len) {
+                               skb_queue_head(&sk->sk_receive_queue, skb);
+                               goto done;
+                       }
+               }
+
+               kfree_skb(skb);
+       }
+
+       /* XXX Queue backlogged skbs */
+
+done:
+       /* SOCK_SEQPACKET: return real length if MSG_TRUNC is set */
+       if (sk->sk_type == SOCK_SEQPACKET && (flags & MSG_TRUNC))
+               copied = rlen;
+
+       return copied;
+}
+
+static const struct proto_ops llcp_sock_ops = {
+       .family         = PF_NFC,
+       .owner          = THIS_MODULE,
+       .bind           = llcp_sock_bind,
+       .connect        = llcp_sock_connect,
+       .release        = llcp_sock_release,
+       .socketpair     = sock_no_socketpair,
+       .accept         = llcp_sock_accept,
+       .getname        = llcp_sock_getname,
+       .poll           = llcp_sock_poll,
+       .ioctl          = sock_no_ioctl,
+       .listen         = llcp_sock_listen,
+       .shutdown       = sock_no_shutdown,
+       .setsockopt     = nfc_llcp_setsockopt,
+       .getsockopt     = nfc_llcp_getsockopt,
+       .sendmsg        = llcp_sock_sendmsg,
+       .recvmsg        = llcp_sock_recvmsg,
+       .mmap           = sock_no_mmap,
+};
+
+static const struct proto_ops llcp_rawsock_ops = {
+       .family         = PF_NFC,
+       .owner          = THIS_MODULE,
+       .bind           = llcp_raw_sock_bind,
+       .connect        = sock_no_connect,
+       .release        = llcp_sock_release,
+       .socketpair     = sock_no_socketpair,
+       .accept         = sock_no_accept,
+       .getname        = llcp_sock_getname,
+       .poll           = llcp_sock_poll,
+       .ioctl          = sock_no_ioctl,
+       .listen         = sock_no_listen,
+       .shutdown       = sock_no_shutdown,
+       .setsockopt     = sock_no_setsockopt,
+       .getsockopt     = sock_no_getsockopt,
+       .sendmsg        = sock_no_sendmsg,
+       .recvmsg        = llcp_sock_recvmsg,
+       .mmap           = sock_no_mmap,
+};
+
+static void llcp_sock_destruct(struct sock *sk)
+{
+       struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk);
+
+       pr_debug("%p\n", sk);
+
+       if (sk->sk_state == LLCP_CONNECTED)
+               nfc_put_device(llcp_sock->dev);
+
+       skb_queue_purge(&sk->sk_receive_queue);
+
+       nfc_llcp_sock_free(llcp_sock);
+
+       if (!sock_flag(sk, SOCK_DEAD)) {
+               pr_err("Freeing alive NFC LLCP socket %p\n", sk);
+               return;
+       }
+}
+
+struct sock *nfc_llcp_sock_alloc(struct socket *sock, int type, gfp_t gfp)
+{
+       struct sock *sk;
+       struct nfc_llcp_sock *llcp_sock;
+
+       sk = sk_alloc(&init_net, PF_NFC, gfp, &llcp_sock_proto);
+       if (!sk)
+               return NULL;
+
+       llcp_sock = nfc_llcp_sock(sk);
+
+       sock_init_data(sock, sk);
+       sk->sk_state = LLCP_CLOSED;
+       sk->sk_protocol = NFC_SOCKPROTO_LLCP;
+       sk->sk_type = type;
+       sk->sk_destruct = llcp_sock_destruct;
+
+       llcp_sock->ssap = 0;
+       llcp_sock->dsap = LLCP_SAP_SDP;
+       llcp_sock->rw = LLCP_MAX_RW + 1;
+       llcp_sock->miux = cpu_to_be16(LLCP_MAX_MIUX + 1);
+       llcp_sock->send_n = llcp_sock->send_ack_n = 0;
+       llcp_sock->recv_n = llcp_sock->recv_ack_n = 0;
+       llcp_sock->remote_ready = 1;
+       llcp_sock->reserved_ssap = LLCP_SAP_MAX;
+       nfc_llcp_socket_remote_param_init(llcp_sock);
+       skb_queue_head_init(&llcp_sock->tx_queue);
+       skb_queue_head_init(&llcp_sock->tx_pending_queue);
+       INIT_LIST_HEAD(&llcp_sock->accept_queue);
+
+       if (sock != NULL)
+               sock->state = SS_UNCONNECTED;
+
+       return sk;
+}
+
+void nfc_llcp_sock_free(struct nfc_llcp_sock *sock)
+{
+       kfree(sock->service_name);
+
+       skb_queue_purge(&sock->tx_queue);
+       skb_queue_purge(&sock->tx_pending_queue);
+
+       list_del_init(&sock->accept_queue);
+
+       sock->parent = NULL;
+
+       nfc_llcp_local_put(sock->local);
+}
+
+static int llcp_sock_create(struct net *net, struct socket *sock,
+                           const struct nfc_protocol *nfc_proto)
+{
+       struct sock *sk;
+
+       pr_debug("%p\n", sock);
+
+       if (sock->type != SOCK_STREAM &&
+           sock->type != SOCK_DGRAM &&
+           sock->type != SOCK_RAW)
+               return -ESOCKTNOSUPPORT;
+
+       if (sock->type == SOCK_RAW)
+               sock->ops = &llcp_rawsock_ops;
+       else
+               sock->ops = &llcp_sock_ops;
+
+       sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC);
+       if (sk == NULL)
+               return -ENOMEM;
+
+       return 0;
+}
+
+static const struct nfc_protocol llcp_nfc_proto = {
+       .id       = NFC_SOCKPROTO_LLCP,
+       .proto    = &llcp_sock_proto,
+       .owner    = THIS_MODULE,
+       .create   = llcp_sock_create
+};
+
+int __init nfc_llcp_sock_init(void)
+{
+       return nfc_proto_register(&llcp_nfc_proto);
+}
+
+void nfc_llcp_sock_exit(void)
+{
+       nfc_proto_unregister(&llcp_nfc_proto);
+}
index 73fd51098f4dc1250ecc89fe160f56055b635987..f0c4d61f37c0f623df3eb40a1222a87f9c875f69 100644 (file)
@@ -28,8 +28,7 @@
 #include <linux/slab.h>
 
 #include "nfc.h"
-
-#include "llcp/llcp.h"
+#include "llcp.h"
 
 static struct genl_multicast_group nfc_genl_event_mcgrp = {
        .name = NFC_GENL_MCAST_EVENT_NAME,
index 94bfe19ba678186cb7467ae03213f07e638046dd..afa1f84ba0406c6bb7622043302a2a6d9bb04703 100644 (file)
@@ -48,8 +48,6 @@ struct nfc_rawsock {
 
 struct nfc_llcp_sdp_tlv;
 
-#ifdef CONFIG_NFC_LLCP
-
 void nfc_llcp_mac_is_down(struct nfc_dev *dev);
 void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
                        u8 comm_mode, u8 rf_mode);
@@ -64,68 +62,6 @@ void nfc_llcp_exit(void);
 void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);
 void nfc_llcp_free_sdp_tlv_list(struct hlist_head *head);
 
-#else
-
-static inline void nfc_llcp_mac_is_down(struct nfc_dev *dev)
-{
-}
-
-static inline void nfc_llcp_mac_is_up(struct nfc_dev *dev, u32 target_idx,
-                                     u8 comm_mode, u8 rf_mode)
-{
-}
-
-static inline int nfc_llcp_register_device(struct nfc_dev *dev)
-{
-       return 0;
-}
-
-static inline void nfc_llcp_unregister_device(struct nfc_dev *dev)
-{
-}
-
-static inline int nfc_llcp_set_remote_gb(struct nfc_dev *dev,
-                                        u8 *gb, u8 gb_len)
-{
-       return 0;
-}
-
-static inline u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, size_t *gb_len)
-{
-       *gb_len = 0;
-       return NULL;
-}
-
-static inline int nfc_llcp_data_received(struct nfc_dev *dev,
-                                        struct sk_buff *skb)
-{
-       return 0;
-}
-
-static inline struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev)
-{
-       return NULL;
-}
-
-static inline int nfc_llcp_init(void)
-{
-       return 0;
-}
-
-static inline void nfc_llcp_exit(void)
-{
-}
-
-static inline void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)
-{
-}
-
-static inline void nfc_llcp_free_sdp_tlv_list(struct hlist_head *sdp_head)
-{
-}
-
-#endif
-
 int __init rawsock_init(void);
 void rawsock_exit(void);