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,
.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)
{
.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)
{
.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);
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)
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);
}
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)
static int btmrvl_sdio_download_fw(struct btmrvl_sdio_card *card)
{
- int ret = 0;
+ int ret;
u8 fws0;
int pollnum = MAX_POLL_TRIES;
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) {
}
}
+ 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;
}
goto unreg_dev;
}
- msleep(100);
-
btmrvl_sdio_enable_host_int(card);
priv = btmrvl_add_card(card);
#include <linux/module.h>
#include <linux/usb.h>
+#include <linux/firmware.h>
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#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 */
/* 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 */
};
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)
{
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);
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);
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 */
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;
}
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;
}
#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 */
size);
#ifdef CONFIG_IWLWIFI_DEBUG
- if (display) {
+ if (buf) {
if (full_log)
bufsz = capacity * 48;
else
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);
}
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;
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);
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;
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;
*/
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);
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)
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);
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);
}
/*
/* 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)},
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;
}
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;
}
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);
}
}
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
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",
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);
}
}
-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;
}
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;
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);
}
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);
/* 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)
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)) {
}
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;
}
/* 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;
}
/* 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;
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);
}
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;
break;
case L2CAP_CID_LE_DATA:
- l2cap_att_channel(conn, cid, skb);
+ l2cap_att_channel(conn, skb);
break;
case L2CAP_CID_SMP:
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;
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;
menuconfig NFC
depends on NET
+ depends on RFKILL || !RFKILL
tristate "NFC subsystem support"
default n
help
source "net/nfc/nci/Kconfig"
source "net/nfc/hci/Kconfig"
-source "net/nfc/llcp/Kconfig"
source "drivers/nfc/Kconfig"
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
--- /dev/null
+/*
+ * 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);
+++ /dev/null
-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
+++ /dev/null
-/*
- * 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;
-}
+++ /dev/null
-/*
- * 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, <o_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();
-}
+++ /dev/null
-/*
- * 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);
+++ /dev/null
-/*
- * 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);
-}
--- /dev/null
+/*
+ * 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;
+}
--- /dev/null
+/*
+ * 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, <o_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();
+}
--- /dev/null
+/*
+ * 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);
+}
#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,
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);
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);