From 8cb25e14fe80d0fac42412364df573eb3e8e83cc Mon Sep 17 00:00:00 2001 From: Helmut Schaa Date: Thu, 8 Dec 2011 13:11:54 +0100 Subject: ieee80211: Introduce ieee80211_is_first_frag Signed-off-by: Helmut Schaa Signed-off-by: John W. Linville --- include/linux/ieee80211.h | 9 +++++++++ 1 file changed, 9 insertions(+) (limited to 'include/linux') diff --git a/include/linux/ieee80211.h b/include/linux/ieee80211.h index 17f2a768e2ad..210e2c325534 100644 --- a/include/linux/ieee80211.h +++ b/include/linux/ieee80211.h @@ -544,6 +544,15 @@ static inline int ieee80211_is_qos_nullfunc(__le16 fc) cpu_to_le16(IEEE80211_FTYPE_DATA | IEEE80211_STYPE_QOS_NULLFUNC); } +/** + * ieee80211_is_first_frag - check if IEEE80211_SCTL_FRAG is not set + * @seq_ctrl: frame sequence control bytes in little-endian byteorder + */ +static inline int ieee80211_is_first_frag(__le16 seq_ctrl) +{ + return (seq_ctrl & cpu_to_le16(IEEE80211_SCTL_FRAG)) == 0; +} + struct ieee80211s_hdr { u8 flags; u8 ttl; -- cgit v1.2.3-59-g8ed1b From 8a5ac6ecd56756ee72588627aa23ab6cf9b790db Mon Sep 17 00:00:00 2001 From: Rafał Miłecki Date: Thu, 8 Dec 2011 18:02:21 +0100 Subject: ssb: extract FEM info from SPROM MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rafał Miłecki Signed-off-by: John W. Linville --- drivers/ssb/pci.c | 23 +++++++++++++++++++++++ include/linux/ssb/ssb.h | 9 +++++++++ include/linux/ssb/ssb_regs.h | 17 +++++++++++++++++ 3 files changed, 49 insertions(+) (limited to 'include/linux') diff --git a/drivers/ssb/pci.c b/drivers/ssb/pci.c index 34c3bab90b9a..973223f5de8e 100644 --- a/drivers/ssb/pci.c +++ b/drivers/ssb/pci.c @@ -607,6 +607,29 @@ static void sprom_extract_r8(struct ssb_sprom *out, const u16 *in) memcpy(&out->antenna_gain.ghz5, &out->antenna_gain.ghz24, sizeof(out->antenna_gain.ghz5)); + /* Extract FEM info */ + SPEX(fem.ghz2.tssipos, SSB_SPROM8_FEM2G, + SSB_SROM8_FEM_TSSIPOS, SSB_SROM8_FEM_TSSIPOS_SHIFT); + SPEX(fem.ghz2.extpa_gain, SSB_SPROM8_FEM2G, + SSB_SROM8_FEM_EXTPA_GAIN, SSB_SROM8_FEM_EXTPA_GAIN_SHIFT); + SPEX(fem.ghz2.pdet_range, SSB_SPROM8_FEM2G, + SSB_SROM8_FEM_PDET_RANGE, SSB_SROM8_FEM_PDET_RANGE_SHIFT); + SPEX(fem.ghz2.tr_iso, SSB_SPROM8_FEM2G, + SSB_SROM8_FEM_TR_ISO, SSB_SROM8_FEM_TR_ISO_SHIFT); + SPEX(fem.ghz2.antswlut, SSB_SPROM8_FEM2G, + SSB_SROM8_FEM_ANTSWLUT, SSB_SROM8_FEM_ANTSWLUT_SHIFT); + + SPEX(fem.ghz5.tssipos, SSB_SPROM8_FEM5G, + SSB_SROM8_FEM_TSSIPOS, SSB_SROM8_FEM_TSSIPOS_SHIFT); + SPEX(fem.ghz5.extpa_gain, SSB_SPROM8_FEM5G, + SSB_SROM8_FEM_EXTPA_GAIN, SSB_SROM8_FEM_EXTPA_GAIN_SHIFT); + SPEX(fem.ghz5.pdet_range, SSB_SPROM8_FEM5G, + SSB_SROM8_FEM_PDET_RANGE, SSB_SROM8_FEM_PDET_RANGE_SHIFT); + SPEX(fem.ghz5.tr_iso, SSB_SPROM8_FEM5G, + SSB_SROM8_FEM_TR_ISO, SSB_SROM8_FEM_TR_ISO_SHIFT); + SPEX(fem.ghz5.antswlut, SSB_SPROM8_FEM5G, + SSB_SROM8_FEM_ANTSWLUT, SSB_SROM8_FEM_ANTSWLUT_SHIFT); + sprom_extract_r458(out, in); /* TODO - get remaining rev 8 stuff needed */ diff --git a/include/linux/ssb/ssb.h b/include/linux/ssb/ssb.h index 061e560251b4..dcf35b0f303a 100644 --- a/include/linux/ssb/ssb.h +++ b/include/linux/ssb/ssb.h @@ -94,6 +94,15 @@ struct ssb_sprom { } ghz5; /* 5GHz band */ } antenna_gain; + struct { + struct { + u8 tssipos, extpa_gain, pdet_range, tr_iso, antswlut; + } ghz2; + struct { + u8 tssipos, extpa_gain, pdet_range, tr_iso, antswlut; + } ghz5; + } fem; + /* TODO - add any parameters needed from rev 2, 3, 4, 5 or 8 SPROMs */ }; diff --git a/include/linux/ssb/ssb_regs.h b/include/linux/ssb/ssb_regs.h index 98941203a27f..c814ae6eeb22 100644 --- a/include/linux/ssb/ssb_regs.h +++ b/include/linux/ssb/ssb_regs.h @@ -432,6 +432,23 @@ #define SSB_SPROM8_RXPO2G 0x00FF /* 2GHz RX power offset */ #define SSB_SPROM8_RXPO5G 0xFF00 /* 5GHz RX power offset */ #define SSB_SPROM8_RXPO5G_SHIFT 8 +#define SSB_SPROM8_FEM2G 0x00AE +#define SSB_SPROM8_FEM5G 0x00B0 +#define SSB_SROM8_FEM_TSSIPOS 0x0001 +#define SSB_SROM8_FEM_TSSIPOS_SHIFT 0 +#define SSB_SROM8_FEM_EXTPA_GAIN 0x0006 +#define SSB_SROM8_FEM_EXTPA_GAIN_SHIFT 1 +#define SSB_SROM8_FEM_PDET_RANGE 0x00F8 +#define SSB_SROM8_FEM_PDET_RANGE_SHIFT 3 +#define SSB_SROM8_FEM_TR_ISO 0x0700 +#define SSB_SROM8_FEM_TR_ISO_SHIFT 8 +#define SSB_SROM8_FEM_ANTSWLUT 0xF800 +#define SSB_SROM8_FEM_ANTSWLUT_SHIFT 11 +#define SSB_SPROM8_THERMAL 0x00B2 +#define SSB_SPROM8_MPWR_RAWTS 0x00B4 +#define SSB_SPROM8_TS_SLP_OPT_CORRX 0x00B6 +#define SSB_SPROM8_FOC_HWIQ_IQSWP 0x00B8 +#define SSB_SPROM8_PHYCAL_TEMPDELTA 0x00BA #define SSB_SPROM8_MAXP_BG 0x00C0 /* Max Power 2GHz in path 1 */ #define SSB_SPROM8_MAXP_BG_MASK 0x00FF /* Mask for Max Power 2GHz */ #define SSB_SPROM8_ITSSI_BG 0xFF00 /* Mask for path 1 itssi_bg */ -- cgit v1.2.3-59-g8ed1b From aee5ed563d56c713d2a51d6f16e08b83fd9665d5 Mon Sep 17 00:00:00 2001 From: Rafał Miłecki Date: Thu, 8 Dec 2011 18:02:22 +0100 Subject: bcma: extract FEM info from SPROM MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Rafał Miłecki Signed-off-by: John W. Linville --- drivers/bcma/sprom.c | 22 ++++++++++++++++++++++ include/linux/bcma/bcma_driver_chipcommon.h | 1 + 2 files changed, 23 insertions(+) (limited to 'include/linux') diff --git a/drivers/bcma/sprom.c b/drivers/bcma/sprom.c index d7292390d236..b6c474bbd572 100644 --- a/drivers/bcma/sprom.c +++ b/drivers/bcma/sprom.c @@ -142,6 +142,28 @@ static void bcma_sprom_extract_r8(struct bcma_bus *bus, const u16 *sprom) bus->sprom.boardflags2_hi = sprom[SPOFF(SSB_SPROM8_BFL2HI)]; bus->sprom.country_code = sprom[SPOFF(SSB_SPROM8_CCODE)]; + + bus->sprom.fem.ghz2.tssipos = (sprom[SPOFF(SSB_SPROM8_FEM2G)] & + SSB_SROM8_FEM_TSSIPOS) >> SSB_SROM8_FEM_TSSIPOS_SHIFT; + bus->sprom.fem.ghz2.extpa_gain = (sprom[SPOFF(SSB_SPROM8_FEM2G)] & + SSB_SROM8_FEM_EXTPA_GAIN) >> SSB_SROM8_FEM_EXTPA_GAIN_SHIFT; + bus->sprom.fem.ghz2.pdet_range = (sprom[SPOFF(SSB_SPROM8_FEM2G)] & + SSB_SROM8_FEM_PDET_RANGE) >> SSB_SROM8_FEM_PDET_RANGE_SHIFT; + bus->sprom.fem.ghz2.tr_iso = (sprom[SPOFF(SSB_SPROM8_FEM2G)] & + SSB_SROM8_FEM_TR_ISO) >> SSB_SROM8_FEM_TR_ISO_SHIFT; + bus->sprom.fem.ghz2.antswlut = (sprom[SPOFF(SSB_SPROM8_FEM2G)] & + SSB_SROM8_FEM_ANTSWLUT) >> SSB_SROM8_FEM_ANTSWLUT_SHIFT; + + bus->sprom.fem.ghz5.tssipos = (sprom[SPOFF(SSB_SPROM8_FEM5G)] & + SSB_SROM8_FEM_TSSIPOS) >> SSB_SROM8_FEM_TSSIPOS_SHIFT; + bus->sprom.fem.ghz5.extpa_gain = (sprom[SPOFF(SSB_SPROM8_FEM5G)] & + SSB_SROM8_FEM_EXTPA_GAIN) >> SSB_SROM8_FEM_EXTPA_GAIN_SHIFT; + bus->sprom.fem.ghz5.pdet_range = (sprom[SPOFF(SSB_SPROM8_FEM5G)] & + SSB_SROM8_FEM_PDET_RANGE) >> SSB_SROM8_FEM_PDET_RANGE_SHIFT; + bus->sprom.fem.ghz5.tr_iso = (sprom[SPOFF(SSB_SPROM8_FEM5G)] & + SSB_SROM8_FEM_TR_ISO) >> SSB_SROM8_FEM_TR_ISO_SHIFT; + bus->sprom.fem.ghz5.antswlut = (sprom[SPOFF(SSB_SPROM8_FEM5G)] & + SSB_SROM8_FEM_ANTSWLUT) >> SSB_SROM8_FEM_ANTSWLUT_SHIFT; } int bcma_sprom_get(struct bcma_bus *bus) diff --git a/include/linux/bcma/bcma_driver_chipcommon.h b/include/linux/bcma/bcma_driver_chipcommon.h index 1526d965ed06..a33086a7530b 100644 --- a/include/linux/bcma/bcma_driver_chipcommon.h +++ b/include/linux/bcma/bcma_driver_chipcommon.h @@ -203,6 +203,7 @@ #define BCMA_CC_PMU_CTL 0x0600 /* PMU control */ #define BCMA_CC_PMU_CTL_ILP_DIV 0xFFFF0000 /* ILP div mask */ #define BCMA_CC_PMU_CTL_ILP_DIV_SHIFT 16 +#define BCMA_CC_PMU_CTL_PLL_UPD 0x00000400 #define BCMA_CC_PMU_CTL_NOILPONW 0x00000200 /* No ILP on wait */ #define BCMA_CC_PMU_CTL_HTREQEN 0x00000100 /* HT req enable */ #define BCMA_CC_PMU_CTL_ALPREQEN 0x00000080 /* ALP req enable */ -- cgit v1.2.3-59-g8ed1b From 9d08f10d355afd500310738ff09b4d921a447102 Mon Sep 17 00:00:00 2001 From: Arend van Spriel Date: Thu, 8 Dec 2011 15:06:41 -0800 Subject: bcma: add set/mask macros for 16-bit register access The BCMA header only had definitions for 32-bit register access. Used those as a template for the 16-bit flavour. Also changed them to inline functions to be on the safe side. As offset parameter is used twice there would be a problem when used like this: bcma_set32(core, offset++, val); Reviewed-by: Pieter-Paul Giesberts Reviewed-by: Alwin Beukers Signed-off-by: Arend van Spriel Signed-off-by: Franky Lin Signed-off-by: John W. Linville --- include/linux/bcma/bcma.h | 32 ++++++++++++++++++++++++++------ 1 file changed, 26 insertions(+), 6 deletions(-) (limited to 'include/linux') diff --git a/include/linux/bcma/bcma.h b/include/linux/bcma/bcma.h index 4d4b59de9467..de6057f16987 100644 --- a/include/linux/bcma/bcma.h +++ b/include/linux/bcma/bcma.h @@ -254,12 +254,32 @@ void bcma_awrite32(struct bcma_device *core, u16 offset, u32 value) core->bus->ops->awrite32(core, offset, value); } -#define bcma_mask32(cc, offset, mask) \ - bcma_write32(cc, offset, bcma_read32(cc, offset) & (mask)) -#define bcma_set32(cc, offset, set) \ - bcma_write32(cc, offset, bcma_read32(cc, offset) | (set)) -#define bcma_maskset32(cc, offset, mask, set) \ - bcma_write32(cc, offset, (bcma_read32(cc, offset) & (mask)) | (set)) +static inline void bcma_mask32(struct bcma_device *cc, u16 offset, u32 mask) +{ + bcma_write32(cc, offset, bcma_read32(cc, offset) & mask); +} +static inline void bcma_set32(struct bcma_device *cc, u16 offset, u32 set) +{ + bcma_write32(cc, offset, bcma_read32(cc, offset) | set); +} +static inline void bcma_maskset32(struct bcma_device *cc, + u16 offset, u32 mask, u32 set) +{ + bcma_write32(cc, offset, (bcma_read32(cc, offset) & mask) | set); +} +static inline void bcma_mask16(struct bcma_device *cc, u16 offset, u16 mask) +{ + bcma_write16(cc, offset, bcma_read16(cc, offset) & mask); +} +static inline void bcma_set16(struct bcma_device *cc, u16 offset, u16 set) +{ + bcma_write16(cc, offset, bcma_read16(cc, offset) | set); +} +static inline void bcma_maskset16(struct bcma_device *cc, + u16 offset, u16 mask, u16 set) +{ + bcma_write16(cc, offset, (bcma_read16(cc, offset) & mask) | set); +} extern bool bcma_core_is_enabled(struct bcma_device *core); extern void bcma_core_disable(struct bcma_device *core, u32 flags); -- cgit v1.2.3-59-g8ed1b From 084455524f0d46dd210b4397898aff73579b97e8 Mon Sep 17 00:00:00 2001 From: Arend van Spriel Date: Thu, 8 Dec 2011 15:06:42 -0800 Subject: bcma: use static keyword for inline function declaration in bcma.h Just scratching an itch here, but it makes more sense to use the static keyword if you think about how the compiler treats inline functions. Reviewed-by: Pieter-Paul Giesberts Reviewed-by: Alwin Beukers Signed-off-by: Arend van Spriel Signed-off-by: Franky Lin Signed-off-by: John W. Linville --- include/linux/bcma/bcma.h | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) (limited to 'include/linux') diff --git a/include/linux/bcma/bcma.h b/include/linux/bcma/bcma.h index de6057f16987..f4b8346b1a33 100644 --- a/include/linux/bcma/bcma.h +++ b/include/linux/bcma/bcma.h @@ -205,50 +205,51 @@ struct bcma_bus { struct ssb_sprom sprom; }; -extern inline u32 bcma_read8(struct bcma_device *core, u16 offset) +static inline u32 bcma_read8(struct bcma_device *core, u16 offset) { return core->bus->ops->read8(core, offset); } -extern inline u32 bcma_read16(struct bcma_device *core, u16 offset) +static inline u32 bcma_read16(struct bcma_device *core, u16 offset) { return core->bus->ops->read16(core, offset); } -extern inline u32 bcma_read32(struct bcma_device *core, u16 offset) +static inline u32 bcma_read32(struct bcma_device *core, u16 offset) { return core->bus->ops->read32(core, offset); } -extern inline +static inline void bcma_write8(struct bcma_device *core, u16 offset, u32 value) { core->bus->ops->write8(core, offset, value); } -extern inline +static inline void bcma_write16(struct bcma_device *core, u16 offset, u32 value) { core->bus->ops->write16(core, offset, value); } -extern inline +static inline void bcma_write32(struct bcma_device *core, u16 offset, u32 value) { core->bus->ops->write32(core, offset, value); } #ifdef CONFIG_BCMA_BLOCKIO -extern inline void bcma_block_read(struct bcma_device *core, void *buffer, +static inline void bcma_block_read(struct bcma_device *core, void *buffer, size_t count, u16 offset, u8 reg_width) { core->bus->ops->block_read(core, buffer, count, offset, reg_width); } -extern inline void bcma_block_write(struct bcma_device *core, const void *buffer, - size_t count, u16 offset, u8 reg_width) +static inline void bcma_block_write(struct bcma_device *core, + const void *buffer, size_t count, + u16 offset, u8 reg_width) { core->bus->ops->block_write(core, buffer, count, offset, reg_width); } #endif -extern inline u32 bcma_aread32(struct bcma_device *core, u16 offset) +static inline u32 bcma_aread32(struct bcma_device *core, u16 offset) { return core->bus->ops->aread32(core, offset); } -extern inline +static inline void bcma_awrite32(struct bcma_device *core, u16 offset, u32 value) { core->bus->ops->awrite32(core, offset, value); -- cgit v1.2.3-59-g8ed1b From 1ed28f610653e9b18433c6d87e9d333b7e3e886e Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Wed, 14 Dec 2011 16:43:09 +0100 Subject: NFC: Add a DEP link control netlink command NFC-DEP (Data Exchange Protocol) is an NFC MAC layer. This command allows to enable and disable the DEP link on to which e.g. LLCP can run. Signed-off-by: Samuel Ortiz Signed-off-by: John W. Linville --- include/linux/nfc.h | 14 +++++ include/net/nfc/nfc.h | 11 ++++ net/nfc/core.c | 77 +++++++++++++++++++++++++++ net/nfc/netlink.c | 144 ++++++++++++++++++++++++++++++++++++++++++++++++++ net/nfc/nfc.h | 9 ++++ 5 files changed, 255 insertions(+) (limited to 'include/linux') diff --git a/include/linux/nfc.h b/include/linux/nfc.h index 36cb955b05cc..34d8303111f0 100644 --- a/include/linux/nfc.h +++ b/include/linux/nfc.h @@ -62,6 +62,8 @@ enum nfc_commands { NFC_CMD_GET_DEVICE, NFC_CMD_DEV_UP, NFC_CMD_DEV_DOWN, + NFC_CMD_DEP_LINK_UP, + NFC_CMD_DEP_LINK_DOWN, NFC_CMD_START_POLL, NFC_CMD_STOP_POLL, NFC_CMD_GET_TARGET, @@ -86,6 +88,8 @@ enum nfc_commands { * @NFC_ATTR_TARGET_SENS_RES: NFC-A targets extra information such as NFCID * @NFC_ATTR_TARGET_SEL_RES: NFC-A targets extra information (useful if the * target is not NFC-Forum compliant) + * @NFC_ATTR_COMM_MODE: Passive or active mode + * @NFC_ATTR_RF_MODE: Initiator or target */ enum nfc_attrs { NFC_ATTR_UNSPEC, @@ -95,6 +99,8 @@ enum nfc_attrs { NFC_ATTR_TARGET_INDEX, NFC_ATTR_TARGET_SENS_RES, NFC_ATTR_TARGET_SEL_RES, + NFC_ATTR_COMM_MODE, + NFC_ATTR_RF_MODE, /* private: internal use only */ __NFC_ATTR_AFTER_LAST }; @@ -111,6 +117,14 @@ enum nfc_attrs { #define NFC_PROTO_MAX 6 +/* NFC communication modes */ +#define NFC_COMM_ACTIVE 0 +#define NFC_COMM_PASSIVE 1 + +/* NFC RF modes */ +#define NFC_RF_INITIATOR 0 +#define NFC_RF_TARGET 1 + /* NFC protocols masks used in bitsets */ #define NFC_PROTO_JEWEL_MASK (1 << NFC_PROTO_JEWEL) #define NFC_PROTO_MIFARE_MASK (1 << NFC_PROTO_MIFARE) diff --git a/include/net/nfc/nfc.h b/include/net/nfc/nfc.h index 3a3304c094d7..bf82d292d68c 100644 --- a/include/net/nfc/nfc.h +++ b/include/net/nfc/nfc.h @@ -52,6 +52,9 @@ struct nfc_ops { int (*dev_down)(struct nfc_dev *dev); int (*start_poll)(struct nfc_dev *dev, u32 protocols); void (*stop_poll)(struct nfc_dev *dev); + int (*dep_link_up)(struct nfc_dev *dev, int target_idx, + u8 comm_mode, u8 rf_mode); + int (*dep_link_down)(struct nfc_dev *dev); int (*activate_target)(struct nfc_dev *dev, u32 target_idx, u32 protocol); void (*deactivate_target)(struct nfc_dev *dev, u32 target_idx); @@ -60,6 +63,9 @@ struct nfc_ops { void *cb_context); }; +#define NFC_TARGET_IDX_ANY -1 +#define NFC_MAX_GT_LEN 48 + struct nfc_target { u32 idx; u32 supported_protocols; @@ -83,6 +89,8 @@ struct nfc_dev { bool dev_up; bool polling; bool remote_activated; + bool dep_link_up; + u32 dep_rf_mode; struct nfc_genl_data genl_data; u32 supported_protocols; @@ -165,4 +173,7 @@ struct sk_buff *nfc_alloc_recv_skb(unsigned int size, gfp_t gfp); int nfc_targets_found(struct nfc_dev *dev, struct nfc_target *targets, int ntargets); +int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx, + u8 comm_mode, u8 rf_mode); + #endif /* __NET_NFC_H */ diff --git a/net/nfc/core.c b/net/nfc/core.c index f53f88ada687..785f1f20c7ba 100644 --- a/net/nfc/core.c +++ b/net/nfc/core.c @@ -181,6 +181,83 @@ error: return rc; } +int nfc_dep_link_up(struct nfc_dev *dev, int target_index, + u8 comm_mode, u8 rf_mode) +{ + int rc = 0; + + pr_debug("dev_name=%s comm:%d rf:%d\n", + dev_name(&dev->dev), comm_mode, rf_mode); + + if (!dev->ops->dep_link_up) + return -EOPNOTSUPP; + + device_lock(&dev->dev); + + if (!device_is_registered(&dev->dev)) { + rc = -ENODEV; + goto error; + } + + if (dev->dep_link_up == true) { + rc = -EALREADY; + goto error; + } + + rc = dev->ops->dep_link_up(dev, target_index, comm_mode, rf_mode); + +error: + device_unlock(&dev->dev); + return rc; +} + +int nfc_dep_link_down(struct nfc_dev *dev) +{ + int rc = 0; + + pr_debug("dev_name=%s\n", dev_name(&dev->dev)); + + if (!dev->ops->dep_link_down) + return -EOPNOTSUPP; + + device_lock(&dev->dev); + + if (!device_is_registered(&dev->dev)) { + rc = -ENODEV; + goto error; + } + + if (dev->dep_link_up == false) { + rc = -EALREADY; + goto error; + } + + if (dev->dep_rf_mode == NFC_RF_TARGET) { + rc = -EOPNOTSUPP; + goto error; + } + + rc = dev->ops->dep_link_down(dev); + if (!rc) { + dev->dep_link_up = false; + nfc_genl_dep_link_down_event(dev); + } + +error: + device_unlock(&dev->dev); + return rc; +} + +int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx, + u8 comm_mode, u8 rf_mode) +{ + dev->dep_link_up = true; + dev->dep_rf_mode = rf_mode; + + return nfc_genl_dep_link_up_event(dev, target_idx, comm_mode, rf_mode); +} +EXPORT_SYMBOL(nfc_dep_link_is_up); + /** * nfc_activate_target - prepare the target for data exchange * diff --git a/net/nfc/netlink.c b/net/nfc/netlink.c index 1d76d38c4a24..43a1c47756a7 100644 --- a/net/nfc/netlink.c +++ b/net/nfc/netlink.c @@ -46,6 +46,8 @@ static const struct nla_policy nfc_genl_policy[NFC_ATTR_MAX + 1] = { [NFC_ATTR_DEVICE_NAME] = { .type = NLA_STRING, .len = NFC_DEVICE_NAME_MAXSIZE }, [NFC_ATTR_PROTOCOLS] = { .type = NLA_U32 }, + [NFC_ATTR_COMM_MODE] = { .type = NLA_U8 }, + [NFC_ATTR_RF_MODE] = { .type = NLA_U8 }, }; static int nfc_genl_send_target(struct sk_buff *msg, struct nfc_target *target, @@ -311,6 +313,75 @@ static int nfc_genl_dump_devices_done(struct netlink_callback *cb) return 0; } +int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx, + u8 comm_mode, u8 rf_mode) +{ + struct sk_buff *msg; + void *hdr; + + pr_debug("DEP link is up\n"); + + msg = nlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_CMD_DEP_LINK_UP); + if (!hdr) + goto free_msg; + + NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx); + if (rf_mode == NFC_RF_INITIATOR) + NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx); + NLA_PUT_U8(msg, NFC_ATTR_COMM_MODE, comm_mode); + NLA_PUT_U8(msg, NFC_ATTR_RF_MODE, rf_mode); + + genlmsg_end(msg, hdr); + + dev->dep_link_up = true; + + genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + +int nfc_genl_dep_link_down_event(struct nfc_dev *dev) +{ + struct sk_buff *msg; + void *hdr; + + pr_debug("DEP link is down\n"); + + msg = nlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC); + if (!msg) + return -ENOMEM; + + hdr = genlmsg_put(msg, 0, 0, &nfc_genl_family, 0, + NFC_CMD_DEP_LINK_DOWN); + if (!hdr) + goto free_msg; + + NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, dev->idx); + + genlmsg_end(msg, hdr); + + genlmsg_multicast(msg, 0, nfc_genl_event_mcgrp.id, GFP_ATOMIC); + + return 0; + +nla_put_failure: + genlmsg_cancel(msg, hdr); +free_msg: + nlmsg_free(msg); + return -EMSGSIZE; +} + static int nfc_genl_get_device(struct sk_buff *skb, struct genl_info *info) { struct sk_buff *msg; @@ -398,6 +469,8 @@ static int nfc_genl_start_poll(struct sk_buff *skb, struct genl_info *info) u32 idx; u32 protocols; + pr_debug("Poll start\n"); + if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || !info->attrs[NFC_ATTR_PROTOCOLS]) return -EINVAL; @@ -452,6 +525,67 @@ out: return rc; } +static int nfc_genl_dep_link_up(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + int rc, tgt_idx; + u32 idx; + u8 comm, rf; + + pr_debug("DEP link up\n"); + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX] || + !info->attrs[NFC_ATTR_COMM_MODE] || + !info->attrs[NFC_ATTR_RF_MODE]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + if (!info->attrs[NFC_ATTR_TARGET_INDEX]) + tgt_idx = NFC_TARGET_IDX_ANY; + else + tgt_idx = nla_get_u32(info->attrs[NFC_ATTR_TARGET_INDEX]); + + comm = nla_get_u8(info->attrs[NFC_ATTR_COMM_MODE]); + rf = nla_get_u8(info->attrs[NFC_ATTR_RF_MODE]); + + if (comm != NFC_COMM_ACTIVE && comm != NFC_COMM_PASSIVE) + return -EINVAL; + + if (rf != NFC_RF_INITIATOR && comm != NFC_RF_TARGET) + return -EINVAL; + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + rc = nfc_dep_link_up(dev, tgt_idx, comm, rf); + + nfc_put_device(dev); + + return rc; +} + +static int nfc_genl_dep_link_down(struct sk_buff *skb, struct genl_info *info) +{ + struct nfc_dev *dev; + int rc; + u32 idx; + + if (!info->attrs[NFC_ATTR_DEVICE_INDEX]) + return -EINVAL; + + idx = nla_get_u32(info->attrs[NFC_ATTR_DEVICE_INDEX]); + + dev = nfc_get_device(idx); + if (!dev) + return -ENODEV; + + rc = nfc_dep_link_down(dev); + + nfc_put_device(dev); + return rc; +} + static struct genl_ops nfc_genl_ops[] = { { .cmd = NFC_CMD_GET_DEVICE, @@ -480,6 +614,16 @@ static struct genl_ops nfc_genl_ops[] = { .doit = nfc_genl_stop_poll, .policy = nfc_genl_policy, }, + { + .cmd = NFC_CMD_DEP_LINK_UP, + .doit = nfc_genl_dep_link_up, + .policy = nfc_genl_policy, + }, + { + .cmd = NFC_CMD_DEP_LINK_DOWN, + .doit = nfc_genl_dep_link_down, + .policy = nfc_genl_policy, + }, { .cmd = NFC_CMD_GET_TARGET, .dumpit = nfc_genl_dump_targets, diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h index 67d605015304..4d0fb125d033 100644 --- a/net/nfc/nfc.h +++ b/net/nfc/nfc.h @@ -68,6 +68,10 @@ int nfc_genl_targets_found(struct nfc_dev *dev); int nfc_genl_device_added(struct nfc_dev *dev); int nfc_genl_device_removed(struct nfc_dev *dev); +int nfc_genl_dep_link_up_event(struct nfc_dev *dev, u32 target_idx, + u8 comm_mode, u8 rf_mode); +int nfc_genl_dep_link_down_event(struct nfc_dev *dev); + struct nfc_dev *nfc_get_device(unsigned idx); static inline void nfc_put_device(struct nfc_dev *dev) @@ -102,6 +106,11 @@ int nfc_start_poll(struct nfc_dev *dev, u32 protocols); int nfc_stop_poll(struct nfc_dev *dev); +int nfc_dep_link_up(struct nfc_dev *dev, int target_idx, + u8 comm_mode, u8 rf_mode); + +int nfc_dep_link_down(struct nfc_dev *dev); + int nfc_activate_target(struct nfc_dev *dev, u32 target_idx, u32 protocol); int nfc_deactivate_target(struct nfc_dev *dev, u32 target_idx); -- cgit v1.2.3-59-g8ed1b From d646960f7986fefb460a2b062d5ccc8ccfeacc3a Mon Sep 17 00:00:00 2001 From: Samuel Ortiz Date: Wed, 14 Dec 2011 16:43:12 +0100 Subject: NFC: Initial LLCP support This patch is an initial implementation for the NFC Logical Link Control protocol. It's also known as NFC peer to peer mode. This is a basic implementation as it lacks SDP (services Discovery Protocol), frames aggregation support, and frame rejecion parsing. Follow up patches will implement those missing features. This code has been tested against a Nexus S phone implementing LLCP 1.0. Signed-off-by: Samuel Ortiz Signed-off-by: John W. Linville --- include/linux/nfc.h | 15 +- net/nfc/Kconfig | 1 + net/nfc/Makefile | 1 + net/nfc/core.c | 20 +- net/nfc/llcp/Kconfig | 7 + net/nfc/llcp/commands.c | 399 ++++++++++++++++++++ net/nfc/llcp/llcp.c | 973 ++++++++++++++++++++++++++++++++++++++++++++++++ net/nfc/llcp/llcp.h | 193 ++++++++++ net/nfc/llcp/sock.c | 675 +++++++++++++++++++++++++++++++++ net/nfc/nfc.h | 54 +++ 10 files changed, 2335 insertions(+), 3 deletions(-) create mode 100644 net/nfc/llcp/Kconfig create mode 100644 net/nfc/llcp/commands.c create mode 100644 net/nfc/llcp/llcp.c create mode 100644 net/nfc/llcp/llcp.h create mode 100644 net/nfc/llcp/sock.c (limited to 'include/linux') diff --git a/include/linux/nfc.h b/include/linux/nfc.h index 34d8303111f0..89fee4ab1904 100644 --- a/include/linux/nfc.h +++ b/include/linux/nfc.h @@ -139,9 +139,22 @@ struct sockaddr_nfc { __u32 nfc_protocol; }; +#define NFC_LLCP_MAX_SERVICE_NAME 63 +struct sockaddr_nfc_llcp { + sa_family_t sa_family; + __u32 dev_idx; + __u32 target_idx; + __u32 nfc_protocol; + __u8 dsap; /* Destination SAP, if known */ + __u8 ssap; /* Source SAP to be bound to */ + char service_name[NFC_LLCP_MAX_SERVICE_NAME]; /* Service name URI */; + size_t service_name_len; +}; + /* NFC socket protocols */ #define NFC_SOCKPROTO_RAW 0 -#define NFC_SOCKPROTO_MAX 1 +#define NFC_SOCKPROTO_LLCP 1 +#define NFC_SOCKPROTO_MAX 2 #define NFC_HEADER_SIZE 1 diff --git a/net/nfc/Kconfig b/net/nfc/Kconfig index 58cddadf8e8e..44c865b86d6f 100644 --- a/net/nfc/Kconfig +++ b/net/nfc/Kconfig @@ -14,5 +14,6 @@ menuconfig NFC be called nfc. source "net/nfc/nci/Kconfig" +source "net/nfc/llcp/Kconfig" source "drivers/nfc/Kconfig" diff --git a/net/nfc/Makefile b/net/nfc/Makefile index fbb550f2377b..7b4a6dcfa566 100644 --- a/net/nfc/Makefile +++ b/net/nfc/Makefile @@ -6,3 +6,4 @@ obj-$(CONFIG_NFC) += nfc.o obj-$(CONFIG_NFC_NCI) += nci/ nfc-objs := core.o netlink.o af_nfc.o rawsock.o +nfc-$(CONFIG_NFC_LLCP) += llcp/llcp.o llcp/commands.o llcp/sock.o diff --git a/net/nfc/core.c b/net/nfc/core.c index 3a45f21b3b97..3ddf6e698df0 100644 --- a/net/nfc/core.c +++ b/net/nfc/core.c @@ -240,6 +240,7 @@ int nfc_dep_link_down(struct nfc_dev *dev) rc = dev->ops->dep_link_down(dev); if (!rc) { dev->dep_link_up = false; + nfc_llcp_mac_is_down(dev); nfc_genl_dep_link_down_event(dev); } @@ -254,6 +255,8 @@ int nfc_dep_link_is_up(struct nfc_dev *dev, u32 target_idx, dev->dep_link_up = true; dev->dep_rf_mode = rf_mode; + nfc_llcp_mac_is_up(dev, target_idx, comm_mode, rf_mode); + return nfc_genl_dep_link_up_event(dev, target_idx, comm_mode, rf_mode); } EXPORT_SYMBOL(nfc_dep_link_is_up); @@ -360,13 +363,13 @@ int nfc_set_remote_general_bytes(struct nfc_dev *dev, u8 *gb, u8 gb_len) if (gb_len > NFC_MAX_GT_LEN) return -EINVAL; - return 0; + return nfc_llcp_set_remote_gb(dev, gb, gb_len); } EXPORT_SYMBOL(nfc_set_remote_general_bytes); u8 *nfc_get_local_general_bytes(struct nfc_dev *dev, u8 *gt_len) { - return NULL; + return nfc_llcp_general_bytes(dev, gt_len); } EXPORT_SYMBOL(nfc_get_local_general_bytes); @@ -560,6 +563,10 @@ int nfc_register_device(struct nfc_dev *dev) if (rc < 0) return rc; + rc = nfc_llcp_register_device(dev); + if (rc) + pr_err("Could not register llcp device\n"); + rc = nfc_genl_device_added(dev); if (rc) pr_debug("The userspace won't be notified that the device %s was added\n", @@ -591,6 +598,8 @@ void nfc_unregister_device(struct nfc_dev *dev) mutex_unlock(&nfc_devlist_mutex); + nfc_llcp_unregister_device(dev); + rc = nfc_genl_device_removed(dev); if (rc) pr_debug("The userspace won't be notified that the device %s was removed\n", @@ -620,6 +629,10 @@ static int __init nfc_init(void) if (rc) goto err_rawsock; + rc = nfc_llcp_init(); + if (rc) + goto err_llcp_sock; + rc = af_nfc_init(); if (rc) goto err_af_nfc; @@ -627,6 +640,8 @@ static int __init nfc_init(void) return 0; err_af_nfc: + nfc_llcp_exit(); +err_llcp_sock: rawsock_exit(); err_rawsock: nfc_genl_exit(); @@ -638,6 +653,7 @@ err_genl: static void __exit nfc_exit(void) { af_nfc_exit(); + nfc_llcp_exit(); rawsock_exit(); nfc_genl_exit(); class_unregister(&nfc_class); diff --git a/net/nfc/llcp/Kconfig b/net/nfc/llcp/Kconfig new file mode 100644 index 000000000000..fbf5e8150908 --- /dev/null +++ b/net/nfc/llcp/Kconfig @@ -0,0 +1,7 @@ +config NFC_LLCP + depends on NFC && EXPERIMENTAL + bool "NFC LLCP support (EXPERIMENTAL)" + default n + help + Say Y here if you want to build support for a kernel NFC LLCP + implementation. \ No newline at end of file diff --git a/net/nfc/llcp/commands.c b/net/nfc/llcp/commands.c new file mode 100644 index 000000000000..151f2ef429c4 --- /dev/null +++ b/net/nfc/llcp/commands.c @@ -0,0 +1,399 @@ +/* + * 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 +#include +#include +#include + +#include + +#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 u8 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) & 0x7f; +} + +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 + 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; +} + +int nfc_llcp_parse_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; + case LLCP_TLV_RW: + local->remote_rw = llcp_tlv_rw(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 rw %d\n", + local->remote_version, local->remote_miu, + local->remote_lto, local->remote_opt, + local->remote_wks, local->remote_rw); + + 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; + u16 size = 0; + + pr_debug("Sending DISC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + dev = sock->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_ATOMIC); + if (skb == NULL) + return -ENOMEM; + + skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE); + + skb = llcp_add_header(skb, sock->ssap, sock->dsap, LLCP_PDU_DISC); + + 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); + + 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; + int err; + u16 size = 0; + + 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; + } + + 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_queue_tail(&local->tx_queue, skb); + + return 0; + +error_tlv: + pr_err("error %d\n", err); + + kfree(service_name_tlv); + + return err; +} + +int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) +{ + struct nfc_llcp_local *local; + struct sk_buff *skb; + + pr_debug("Sending CC\n"); + + local = sock->local; + if (local == NULL) + return -ENODEV; + + skb = llcp_allocate_pdu(sock, LLCP_PDU_CC, 0); + if (skb == NULL) + return -ENOMEM; + + 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, ssap, dsap, 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; +} diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c new file mode 100644 index 000000000000..67756b23eac5 --- /dev/null +++ b/net/nfc/llcp/llcp.c @@ -0,0 +1,973 @@ +/* + * 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 +#include +#include +#include + +#include "../nfc.h" +#include "llcp.h" + +static u8 llcp_magic[3] = {0x46, 0x66, 0x6d}; + +static struct list_head llcp_devices; + +static void nfc_llcp_socket_release(struct nfc_llcp_local *local) +{ + struct nfc_llcp_sock *parent, *s, *n; + struct sock *sk, *parent_sk; + int i; + + + mutex_lock(&local->socket_lock); + + for (i = 0; i < LLCP_MAX_SAP; i++) { + parent = local->sockets[i]; + if (parent == NULL) + continue; + + /* Release all child sockets */ + list_for_each_entry_safe(s, n, &parent->list, list) { + list_del(&s->list); + sk = &s->sk; + + lock_sock(sk); + + if (sk->sk_state == LLCP_CONNECTED) + nfc_put_device(s->dev); + + sk->sk_state = LLCP_CLOSED; + sock_set_flag(sk, SOCK_DEAD); + + release_sock(sk); + } + + parent_sk = &parent->sk; + + lock_sock(parent_sk); + + if (parent_sk->sk_state == LLCP_LISTEN) { + struct nfc_llcp_sock *lsk, *n; + struct sock *accept_sk; + + list_for_each_entry_safe(lsk, n, &parent->accept_queue, + accept_queue) { + accept_sk = &lsk->sk; + lock_sock(accept_sk); + + nfc_llcp_accept_unlink(accept_sk); + + accept_sk->sk_state = LLCP_CLOSED; + sock_set_flag(accept_sk, SOCK_DEAD); + + release_sock(accept_sk); + + sock_orphan(accept_sk); + } + } + + if (parent_sk->sk_state == LLCP_CONNECTED) + nfc_put_device(parent->dev); + + parent_sk->sk_state = LLCP_CLOSED; + sock_set_flag(parent_sk, SOCK_DEAD); + + release_sock(parent_sk); + } + + mutex_unlock(&local->socket_lock); +} + +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"); + + queue_work(local->timeout_wq, &local->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; +} + +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(BIT(ssap), &local->local_wks); + mutex_unlock(&local->sdp_lock); + + return ssap; + } + + /* + * This is not a well known service, + * we should try to find a local SDP free spot + */ + 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(BIT(ssap), &local->local_sdp); + mutex_unlock(&local->sdp_lock); + + return LLCP_WKS_NUM_SAP + ssap; + + } else if (sock->ssap != 0) { + if (sock->ssap < LLCP_WKS_NUM_SAP) { + if (!(local->local_wks & BIT(sock->ssap))) { + set_bit(BIT(sock->ssap), &local->local_wks); + mutex_unlock(&local->sdp_lock); + + return sock->ssap; + } + + } else if (sock->ssap < LLCP_SDP_NUM_SAP) { + if (!(local->local_sdp & + BIT(sock->ssap - LLCP_WKS_NUM_SAP))) { + set_bit(BIT(sock->ssap - LLCP_WKS_NUM_SAP), + &local->local_sdp); + 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(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) { + local_ssap = ssap - LLCP_WKS_NUM_SAP; + sdp = &local->local_sdp; + } 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(1 << local_ssap, sdp); + + mutex_unlock(&local->sdp_lock); +} + +u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len) +{ + struct nfc_llcp_local *local; + + local = nfc_llcp_find_local(dev); + if (local == NULL) { + *general_bytes_len = 0; + return NULL; + } + + *general_bytes_len = local->gb_len; + + return local->gb; +} + +static int nfc_llcp_build_gb(struct nfc_llcp_local *local) +{ + u8 *gb_cur, *version_tlv, version, version_length; + u8 *lto_tlv, lto, lto_length; + u8 *wks_tlv, wks_length; + u8 gb_len = 0; + + version = LLCP_VERSION_11; + version_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, &version, + 1, &version_length); + gb_len += version_length; + + /* 1500 ms */ + lto = 150; + lto_tlv = nfc_llcp_build_tlv(LLCP_TLV_VERSION, <o, 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; + + gb_len += ARRAY_SIZE(llcp_magic); + + if (gb_len > NFC_MAX_GT_LEN) { + kfree(version_tlv); + return -EINVAL; + } + + 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; + + kfree(version_tlv); + kfree(lto_tlv); + + local->gb_len = gb_len; + + return 0; +} + +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; + } + + memset(local->remote_gb, 0, NFC_MAX_GT_LEN); + memcpy(local->remote_gb, gb, gb_len); + local->remote_gb_len = gb_len; + + if (local->remote_gb == NULL || + local->remote_gb_len == 0) + return -ENODEV; + + if (memcmp(local->remote_gb, llcp_magic, 3)) { + pr_err("MAC does not support LLCP\n"); + return -EINVAL; + } + + return nfc_llcp_parse_tlv(local, + &local->remote_gb[3], local->remote_gb_len - 3); +} + +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; + + skb = skb_dequeue(&local->tx_queue); + if (skb != NULL) { + pr_debug("Sending pending skb\n"); + nfc_data_exchange(local->dev, local->target_idx, + skb, nfc_llcp_recv, local); + } else { + nfc_llcp_send_symm(local->dev); + } + + mod_timer(&local->link_timer, + jiffies + msecs_to_jiffies(local->remote_lto)); +} + +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 - 1) % 16); + sock->send_n = (sock->send_n + 1) % 16; + sock->recv_ack_n = (sock->recv_n - 1) % 16; +} + +static struct nfc_llcp_sock *nfc_llcp_sock_get(struct nfc_llcp_local *local, + u8 ssap, u8 dsap) +{ + struct nfc_llcp_sock *sock, *llcp_sock, *n; + + if (ssap == 0 && dsap == 0) + return NULL; + + mutex_lock(&local->socket_lock); + sock = local->sockets[ssap]; + if (sock == NULL) { + mutex_unlock(&local->socket_lock); + return NULL; + } + + pr_debug("root dsap %d (%d)\n", sock->dsap, dsap); + + if (sock->dsap == dsap) { + sock_hold(&sock->sk); + mutex_unlock(&local->socket_lock); + return sock; + } + + list_for_each_entry_safe(llcp_sock, n, &sock->list, list) { + pr_debug("llcp_sock %p sk %p dsap %d\n", llcp_sock, + &llcp_sock->sk, llcp_sock->dsap); + if (llcp_sock->dsap == dsap) { + sock_hold(&llcp_sock->sk); + mutex_unlock(&local->socket_lock); + return llcp_sock; + } + } + + pr_err("Could not find socket for %d %d\n", ssap, dsap); + + mutex_unlock(&local->socket_lock); + + return NULL; +} + +static void nfc_llcp_sock_put(struct nfc_llcp_sock *sock) +{ + sock_put(&sock->sk); +} + +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_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, bound_sap, reason; + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + pr_debug("%d %d\n", dsap, ssap); + + nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], + skb->len - LLCP_HEADER_SIZE); + + if (dsap != LLCP_SAP_SDP) { + bound_sap = dsap; + + mutex_lock(&local->socket_lock); + sock = local->sockets[dsap]; + if (sock == NULL) { + mutex_unlock(&local->socket_lock); + reason = LLCP_DM_NOBOUND; + goto fail; + } + + sock_hold(&sock->sk); + mutex_unlock(&local->socket_lock); + + lock_sock(&sock->sk); + + if (sock->dsap == LLCP_SAP_SDP && + sock->sk.sk_state == LLCP_LISTEN) + goto enqueue; + } 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); + + mutex_lock(&local->socket_lock); + for (bound_sap = 0; bound_sap < LLCP_LOCAL_SAP_OFFSET; + bound_sap++) { + sock = local->sockets[bound_sap]; + if (sock == NULL) + continue; + + if (sock->service_name == NULL || + sock->service_name_len == 0) + continue; + + if (sock->service_name_len != sn_len) + continue; + + if (sock->dsap == LLCP_SAP_SDP && + sock->sk.sk_state == LLCP_LISTEN && + !memcmp(sn, sock->service_name, sn_len)) { + pr_debug("Found service name at SAP %d\n", + bound_sap); + sock_hold(&sock->sk); + mutex_unlock(&local->socket_lock); + + lock_sock(&sock->sk); + + goto enqueue; + } + } + + } + + mutex_unlock(&local->socket_lock); + + reason = LLCP_DM_NOBOUND; + goto fail; + +enqueue: + parent = &sock->sk; + + if (sk_acceptq_is_full(parent)) { + reason = LLCP_DM_REJ; + release_sock(&sock->sk); + sock_put(&sock->sk); + goto fail; + } + + 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 = local; + new_sock->nfc_protocol = sock->nfc_protocol; + new_sock->ssap = bound_sap; + new_sock->dsap = ssap; + new_sock->parent = parent; + + pr_debug("new sock %p sk %p\n", new_sock, &new_sock->sk); + + list_add_tail(&new_sock->list, &sock->list); + + 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); + + return; + +} + +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); + } + + 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"); + + /* Pass the payload upstream */ + if (ptype == LLCP_PDU_I) { + pr_debug("I frame, queueing on %p\n", &llcp_sock->sk); + + skb_pull(skb, LLCP_HEADER_SIZE + LLCP_SEQUENCE_SIZE); + if (sock_queue_rcv_skb(&llcp_sock->sk, skb)) { + pr_err("receive queue is full\n"); + skb_queue_head(&llcp_sock->tx_backlog_queue, skb); + } + } + + /* Remove skbs from the pending queue */ + if (llcp_sock->send_ack_n != nr) { + struct sk_buff *s, *tmp; + + llcp_sock->send_ack_n = nr; + + skb_queue_walk_safe(&llcp_sock->tx_pending_queue, s, tmp) + if (nfc_llcp_ns(s) <= nr) { + skb_unlink(s, &llcp_sock->tx_pending_queue); + kfree_skb(s); + } + } + + /* Queue some I frames for transmission */ + while (llcp_sock->remote_ready && + skb_queue_len(&llcp_sock->tx_pending_queue) <= local->remote_rw) { + struct sk_buff *pdu, *pending_pdu; + + pdu = skb_dequeue(&llcp_sock->tx_queue); + if (pdu == NULL) + break; + + /* Update N(S)/N(R) */ + nfc_llcp_set_nrns(llcp_sock, pdu); + + pending_pdu = skb_clone(pdu, GFP_KERNEL); + + skb_queue_tail(&local->tx_queue, pdu); + skb_queue_tail(&llcp_sock->tx_pending_queue, pending_pdu); + } + + 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); + + 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); + } + + + 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; + u8 dsap, ssap; + + + dsap = nfc_llcp_dsap(skb); + ssap = nfc_llcp_ssap(skb); + + llcp_sock = nfc_llcp_sock_get(local, dsap, ssap); + + if (llcp_sock == NULL) + llcp_sock = nfc_llcp_sock_get(local, dsap, LLCP_SAP_SDP); + + if (llcp_sock == NULL) { + pr_err("Invalid CC\n"); + nfc_llcp_send_dm(local, dsap, ssap, LLCP_DM_NOCONN); + + return; + } + + llcp_sock->dsap = ssap; + + nfc_llcp_parse_tlv(local, &skb->data[LLCP_HEADER_SIZE], + skb->len - LLCP_HEADER_SIZE); + + nfc_llcp_sock_put(llcp_sock); +} + +static void nfc_llcp_rx_work(struct work_struct *work) +{ + struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local, + rx_work); + u8 dsap, ssap, ptype; + struct sk_buff *skb; + + skb = local->rx_pending; + if (skb == NULL) { + pr_debug("No pending SKB\n"); + return; + } + + 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); + + switch (ptype) { + case LLCP_PDU_SYMM: + pr_debug("SYMM\n"); + 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_I: + case LLCP_PDU_RR: + pr_debug("I frame\n"); + nfc_llcp_recv_hdlc(local, skb); + break; + + } + + queue_work(local->tx_wq, &local->tx_work); + kfree_skb(local->rx_pending); + local->rx_pending = NULL; + + return; +} + +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", err); + return; + } + + local->rx_pending = skb_get(skb); + del_timer(&local->link_timer); + queue_work(local->rx_wq, &local->rx_work); + + return; +} + +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; + + /* Close and purge all existing sockets */ + nfc_llcp_socket_release(local); +} + +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"); + + queue_work(local->tx_wq, &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 device *dev = &ndev->dev; + struct nfc_llcp_local *local; + char name[32]; + int err; + + local = kzalloc(sizeof(struct nfc_llcp_local), GFP_KERNEL); + if (local == NULL) + return -ENOMEM; + + local->dev = ndev; + INIT_LIST_HEAD(&local->list); + mutex_init(&local->sdp_lock); + mutex_init(&local->socket_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); + snprintf(name, sizeof(name), "%s_llcp_tx_wq", dev_name(dev)); + local->tx_wq = alloc_workqueue(name, + WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); + if (local->tx_wq == NULL) { + err = -ENOMEM; + goto err_local; + } + + local->rx_pending = NULL; + INIT_WORK(&local->rx_work, nfc_llcp_rx_work); + snprintf(name, sizeof(name), "%s_llcp_rx_wq", dev_name(dev)); + local->rx_wq = alloc_workqueue(name, + WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); + if (local->rx_wq == NULL) { + err = -ENOMEM; + goto err_tx_wq; + } + + INIT_WORK(&local->timeout_work, nfc_llcp_timeout_work); + snprintf(name, sizeof(name), "%s_llcp_timeout_wq", dev_name(dev)); + local->timeout_wq = alloc_workqueue(name, + WQ_NON_REENTRANT | WQ_UNBOUND | WQ_MEM_RECLAIM, 1); + if (local->timeout_wq == NULL) { + err = -ENOMEM; + goto err_rx_wq; + } + + nfc_llcp_build_gb(local); + + local->remote_miu = LLCP_DEFAULT_MIU; + local->remote_lto = LLCP_DEFAULT_LTO; + local->remote_rw = LLCP_DEFAULT_RW; + + list_add(&llcp_devices, &local->list); + + return 0; + +err_rx_wq: + destroy_workqueue(local->rx_wq); + +err_tx_wq: + destroy_workqueue(local->tx_wq); + +err_local: + kfree(local); + + 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; + } + + list_del(&local->list); + nfc_llcp_socket_release(local); + del_timer_sync(&local->link_timer); + skb_queue_purge(&local->tx_queue); + destroy_workqueue(local->tx_wq); + destroy_workqueue(local->rx_wq); + kfree(local->rx_pending); + kfree(local); +} + +int __init nfc_llcp_init(void) +{ + INIT_LIST_HEAD(&llcp_devices); + + return nfc_llcp_sock_init(); +} + +void nfc_llcp_exit(void) +{ + nfc_llcp_sock_exit(); +} diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h new file mode 100644 index 000000000000..0ad2e3361584 --- /dev/null +++ b/net/nfc/llcp/llcp.h @@ -0,0 +1,193 @@ +/* + * 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_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) + +struct nfc_llcp_sock; + +struct nfc_llcp_local { + struct list_head list; + struct nfc_dev *dev; + + struct mutex sdp_lock; + struct mutex socket_lock; + + struct timer_list link_timer; + struct sk_buff_head tx_queue; + struct workqueue_struct *tx_wq; + struct work_struct tx_work; + struct workqueue_struct *rx_wq; + struct work_struct rx_work; + struct sk_buff *rx_pending; + struct workqueue_struct *timeout_wq; + struct work_struct timeout_work; + + u32 target_idx; + u8 rf_mode; + u8 comm_mode; + unsigned long local_wks; /* Well known services */ + unsigned long local_sdp; /* Local services */ + unsigned long local_sap; /* Local SAPs, not available for discovery */ + + /* 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; + u8 remote_rw; + + /* sockets array */ + struct nfc_llcp_sock *sockets[LLCP_MAX_SAP]; +}; + +struct nfc_llcp_sock { + struct sock sk; + struct list_head list; + struct nfc_dev *dev; + struct nfc_llcp_local *local; + u32 target_idx; + u32 nfc_protocol; + + u8 ssap; + u8 dsap; + char *service_name; + size_t service_name_len; + + /* 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; + + struct sk_buff_head tx_queue; + struct sk_buff_head tx_pending_queue; + struct sk_buff_head tx_backlog_queue; + + struct list_head accept_queue; + struct sock *parent; +}; + +#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 + +/* 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 + + +struct nfc_llcp_local *nfc_llcp_find_local(struct nfc_dev *dev); +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); + +/* 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_tlv(struct nfc_llcp_local *local, + 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); +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_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason); +int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock); + +/* Socket API */ +int __init nfc_llcp_sock_init(void); +void nfc_llcp_sock_exit(void); diff --git a/net/nfc/llcp/sock.c b/net/nfc/llcp/sock.c new file mode 100644 index 000000000000..f738ccd535f1 --- /dev/null +++ b/net/nfc/llcp/sock.c @@ -0,0 +1,675 @@ +/* + * 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 +#include +#include +#include + +#include "../nfc.h" +#include "llcp.h" + +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; + + pr_debug("sk %p addr %p family %d\n", sk, addr, addr->sa_family); + + if (!addr || addr->sa_family != AF_NFC) + return -EINVAL; + + 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 = 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_MAX_SAP) + goto put_dev; + + local->sockets[llcp_sock->ssap] = llcp_sock; + + 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_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; +} + +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) { + nfc_llcp_accept_unlink(sk); + if (newsock) + sock_graft(sk, newsock); + + release_sock(sk); + + pr_debug("Returning sk state %d\n", sk->sk_state); + + 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 *addr, + int *len, int peer) +{ + struct sockaddr_nfc_llcp *llcp_addr = (struct sockaddr_nfc_llcp *) addr; + struct sock *sk = sock->sk; + struct nfc_llcp_sock *llcp_sock = nfc_llcp_sock(sk); + + pr_debug("%p\n", sk); + + addr->sa_family = AF_NFC; + *len = sizeof(struct sockaddr_nfc_llcp); + + llcp_addr->dev_idx = llcp_sock->dev->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; + + if (!skb_queue_empty(&sk->sk_receive_queue)) + mask |= POLLIN; + + if (sk->sk_state == LLCP_CLOSED) + mask |= POLLHUP; + + 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); + + if (!sk) + return 0; + + pr_debug("%p\n", sk); + + local = llcp_sock->local; + if (local == NULL) + return -ENODEV; + + mutex_lock(&local->socket_lock); + + if (llcp_sock == local->sockets[llcp_sock->ssap]) { + local->sockets[llcp_sock->ssap] = NULL; + } else { + struct nfc_llcp_sock *parent, *s, *n; + + parent = local->sockets[llcp_sock->ssap]; + + list_for_each_entry_safe(s, n, &parent->list, list) + if (llcp_sock == s) { + list_del(&s->list); + break; + } + + } + + mutex_unlock(&local->socket_lock); + + 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); + + sock_set_flag(sk, SOCK_DEAD); + sock_orphan(accept_sk); + sock_put(accept_sk); + } + } + + /* Freeing the SAP */ + if ((sk->sk_state == LLCP_CONNECTED + && llcp_sock->ssap > LLCP_LOCAL_SAP_OFFSET) || + sk->sk_state == LLCP_BOUND || + sk->sk_state == LLCP_LISTEN) + nfc_llcp_put_ssap(llcp_sock->local, llcp_sock->ssap); + + sock_set_flag(sk, SOCK_DEAD); + + release_sock(sk); + + sock_orphan(sk); + sock_put(sk); + + return 0; +} + +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) { + pr_err("Invalid socket\n"); + return -EINVAL; + } + + if (addr->service_name_len == 0 && addr->dsap == 0) { + pr_err("Missing service name or dsap\n"); + 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 = local; + llcp_sock->ssap = nfc_llcp_get_local_ssap(local); + if (llcp_sock->ssap == LLCP_SAP_MAX) { + ret = -ENOMEM; + goto put_dev; + } + 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); + + local->sockets[llcp_sock->ssap] = llcp_sock; + + ret = nfc_llcp_send_connect(llcp_sock); + if (ret) + goto put_dev; + + sk->sk_state = LLCP_CONNECTED; + + release_sock(sk); + return 0; + +put_dev: + nfc_put_device(dev); + +error: + release_sock(sk); + return ret; +} + +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); + + 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 (memcpy_toiovec(msg->msg_iov, cskb->data, copied)) { + if (!(flags & MSG_PEEK)) + skb_queue_head(&sk->sk_receive_queue, skb); + return -EFAULT; + } + + /* 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) { + 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 = 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->send_n = llcp_sock->send_ack_n = 0; + llcp_sock->recv_n = llcp_sock->recv_ack_n = 0; + llcp_sock->remote_ready = 1; + skb_queue_head_init(&llcp_sock->tx_queue); + skb_queue_head_init(&llcp_sock->tx_pending_queue); + skb_queue_head_init(&llcp_sock->tx_backlog_queue); + INIT_LIST_HEAD(&llcp_sock->list); + 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); + skb_queue_purge(&sock->tx_backlog_queue); + + list_del_init(&sock->accept_queue); + + sock->parent = NULL; +} + +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) + return -ESOCKTNOSUPPORT; + + sock->ops = &llcp_sock_ops; + + sk = nfc_llcp_sock_alloc(sock, sock->type, GFP_ATOMIC); + if (sk == NULL) + return -ENOMEM; + + return 0; +} + +static const struct nfc_protocol llcp_nfc_proto = { + .id = NFC_SOCKPROTO_LLCP, + .proto = &llcp_sock_proto, + .owner = THIS_MODULE, + .create = llcp_sock_create +}; + +int __init nfc_llcp_sock_init(void) +{ + return nfc_proto_register(&llcp_nfc_proto); +} + +void nfc_llcp_sock_exit(void) +{ + nfc_proto_unregister(&llcp_nfc_proto); +} diff --git a/net/nfc/nfc.h b/net/nfc/nfc.h index 4d0fb125d033..2c2c4015c68b 100644 --- a/net/nfc/nfc.h +++ b/net/nfc/nfc.h @@ -46,6 +46,60 @@ struct nfc_rawsock { #define to_rawsock_sk(_tx_work) \ ((struct sock *) container_of(_tx_work, struct nfc_rawsock, tx_work)) +#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); +int nfc_llcp_register_device(struct nfc_dev *dev); +void nfc_llcp_unregister_device(struct nfc_dev *dev); +int nfc_llcp_set_remote_gb(struct nfc_dev *dev, u8 *gb, u8 gb_len); +u8 *nfc_llcp_general_bytes(struct nfc_dev *dev, u8 *general_bytes_len); +int __init nfc_llcp_init(void); +void nfc_llcp_exit(void); + +#else + +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) +{ +} + +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, u8 *gb_len) +{ + *gb_len = 0; + return NULL; +} + +static inline int nfc_llcp_init(void) +{ + return 0; +} + +static inline void nfc_llcp_exit(void) +{ +} + +#endif + int __init rawsock_init(void); void rawsock_exit(void); -- cgit v1.2.3-59-g8ed1b From bdd90d5e36a55271beb957b3d7ca3e29b2a90207 Mon Sep 17 00:00:00 2001 From: Johannes Berg Date: Wed, 14 Dec 2011 12:20:27 +0100 Subject: cfg80211: validate nl80211 station handling better The nl80211 station handling code is a bit messy and doesn't do a lot of validation. It seems like this could be an issue for drivers that don't use mac80211 to validate everything. As cfg80211 doesn't keep station state, move the validation of allowing supported_rates to change for TDLS only in station mode to mac80211. Signed-off-by: Johannes Berg Signed-off-by: John W. Linville --- include/linux/nl80211.h | 6 +- include/net/cfg80211.h | 7 +- net/mac80211/cfg.c | 8 ++ net/wireless/nl80211.c | 199 +++++++++++++++++++++++++++--------------------- 4 files changed, 131 insertions(+), 89 deletions(-) (limited to 'include/linux') diff --git a/include/linux/nl80211.h b/include/linux/nl80211.h index a18760684fc9..f795cb7dccdd 100644 --- a/include/linux/nl80211.h +++ b/include/linux/nl80211.h @@ -1536,7 +1536,11 @@ enum nl80211_iftype { * @NL80211_STA_FLAG_WME: station is WME/QoS capable * @NL80211_STA_FLAG_MFP: station uses management frame protection * @NL80211_STA_FLAG_AUTHENTICATED: station is authenticated - * @NL80211_STA_FLAG_TDLS_PEER: station is a TDLS peer + * @NL80211_STA_FLAG_TDLS_PEER: station is a TDLS peer -- this flag should + * only be used in managed mode (even in the flags mask). Note that the + * flag can't be changed, it is only valid while adding a station, and + * attempts to change it will silently be ignored (rather than rejected + * as errors.) * @NL80211_STA_FLAG_MAX: highest station flag number currently defined * @__NL80211_STA_FLAG_AFTER_LAST: internal use */ diff --git a/include/net/cfg80211.h b/include/net/cfg80211.h index 150c0ee714c2..5eda5933ae01 100644 --- a/include/net/cfg80211.h +++ b/include/net/cfg80211.h @@ -1346,7 +1346,12 @@ struct cfg80211_gtk_rekey_data { * * @add_station: Add a new station. * @del_station: Remove a station; @mac may be NULL to remove all stations. - * @change_station: Modify a given station. + * @change_station: Modify a given station. Note that flags changes are not much + * validated in cfg80211, in particular the auth/assoc/authorized flags + * might come to the driver in invalid combinations -- make sure to check + * them, also against the existing state! Also, supported_rates changes are + * not checked in station mode -- drivers need to reject (or ignore) them + * for anything but TDLS peers. * @get_station: get station information for the station identified by @mac * @dump_station: dump station callback -- resume dump at index @idx * diff --git a/net/mac80211/cfg.c b/net/mac80211/cfg.c index 393b2a4445b8..944051b43bad 100644 --- a/net/mac80211/cfg.c +++ b/net/mac80211/cfg.c @@ -976,6 +976,14 @@ static int ieee80211_change_station(struct wiphy *wiphy, return -EINVAL; } + /* in station mode, supported rates are only valid with TDLS */ + if (sdata->vif.type == NL80211_IFTYPE_STATION && + params->supported_rates && + !test_sta_flag(sta, WLAN_STA_TDLS_PEER)) { + rcu_read_unlock(); + return -EINVAL; + } + if (params->vlan && params->vlan != sta->sdata->dev) { vlansdata = IEEE80211_DEV_TO_SUB_IF(params->vlan); diff --git a/net/wireless/nl80211.c b/net/wireless/nl80211.c index d86428145c32..b07c4fc4ae22 100644 --- a/net/wireless/nl80211.c +++ b/net/wireless/nl80211.c @@ -2579,6 +2579,9 @@ static int nl80211_set_station(struct sk_buff *skb, struct genl_info *info) params.ht_capa = nla_data(info->attrs[NL80211_ATTR_HT_CAPABILITY]); + if (!rdev->ops->change_station) + return -EOPNOTSUPP; + if (parse_station_flags(info, ¶ms)) return -EINVAL; @@ -2590,73 +2593,84 @@ static int nl80211_set_station(struct sk_buff *skb, struct genl_info *info) params.plink_state = nla_get_u8(info->attrs[NL80211_ATTR_STA_PLINK_STATE]); - params.vlan = get_vlan(info, rdev); - if (IS_ERR(params.vlan)) - return PTR_ERR(params.vlan); - - /* validate settings */ - err = 0; - switch (dev->ieee80211_ptr->iftype) { case NL80211_IFTYPE_AP: case NL80211_IFTYPE_AP_VLAN: case NL80211_IFTYPE_P2P_GO: /* disallow mesh-specific things */ if (params.plink_action) - err = -EINVAL; + return -EINVAL; + + /* TDLS can't be set, ... */ + if (params.sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER)) + return -EINVAL; + /* + * ... but don't bother the driver with it. This works around + * a hostapd/wpa_supplicant issue -- it always includes the + * TLDS_PEER flag in the mask even for AP mode. + */ + params.sta_flags_mask &= ~BIT(NL80211_STA_FLAG_TDLS_PEER); + + /* accept only the listed bits */ + if (params.sta_flags_mask & + ~(BIT(NL80211_STA_FLAG_AUTHORIZED) | + BIT(NL80211_STA_FLAG_SHORT_PREAMBLE) | + BIT(NL80211_STA_FLAG_WME) | + BIT(NL80211_STA_FLAG_MFP))) + return -EINVAL; + + /* must be last in here for error handling */ + params.vlan = get_vlan(info, rdev); + if (IS_ERR(params.vlan)) + return PTR_ERR(params.vlan); break; case NL80211_IFTYPE_P2P_CLIENT: case NL80211_IFTYPE_STATION: /* disallow things sta doesn't support */ if (params.plink_action) - err = -EINVAL; - if (params.vlan) - err = -EINVAL; - if (params.supported_rates && - !(params.sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER))) - err = -EINVAL; + return -EINVAL; if (params.ht_capa) - err = -EINVAL; + return -EINVAL; if (params.listen_interval >= 0) - err = -EINVAL; - if (params.sta_flags_mask & - ~(BIT(NL80211_STA_FLAG_AUTHORIZED) | - BIT(NL80211_STA_FLAG_TDLS_PEER))) - err = -EINVAL; - /* can't change the TDLS bit */ - if (!(params.sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER)) && - (params.sta_flags_mask & BIT(NL80211_STA_FLAG_TDLS_PEER))) - err = -EINVAL; + return -EINVAL; + /* + * Don't allow userspace to change the TDLS_PEER flag, + * but silently ignore attempts to change it since we + * don't have state here to verify that it doesn't try + * to change the flag. + */ + params.sta_flags_mask &= ~BIT(NL80211_STA_FLAG_TDLS_PEER); + + /* reject any changes other than AUTHORIZED */ + if (params.sta_flags_mask & ~BIT(NL80211_STA_FLAG_AUTHORIZED)) + return -EINVAL; break; case NL80211_IFTYPE_MESH_POINT: /* disallow things mesh doesn't support */ if (params.vlan) - err = -EINVAL; + return -EINVAL; if (params.ht_capa) - err = -EINVAL; + return -EINVAL; if (params.listen_interval >= 0) - err = -EINVAL; + return -EINVAL; + /* + * No special handling for TDLS here -- the userspace + * mesh code doesn't have this bug. + */ if (params.sta_flags_mask & ~(BIT(NL80211_STA_FLAG_AUTHENTICATED) | BIT(NL80211_STA_FLAG_MFP) | BIT(NL80211_STA_FLAG_AUTHORIZED))) - err = -EINVAL; + return -EINVAL; break; default: - err = -EINVAL; + return -EOPNOTSUPP; } - if (err) - goto out; - - if (!rdev->ops->change_station) { - err = -EOPNOTSUPP; - goto out; - } + /* be aware of params.vlan when changing code here */ err = rdev->ops->change_station(&rdev->wiphy, dev, mac_addr, ¶ms); - out: if (params.vlan) dev_put(params.vlan); @@ -2711,70 +2725,81 @@ static int nl80211_new_station(struct sk_buff *skb, struct genl_info *info) params.plink_action = nla_get_u8(info->attrs[NL80211_ATTR_STA_PLINK_ACTION]); + if (!rdev->ops->add_station) + return -EOPNOTSUPP; + if (parse_station_flags(info, ¶ms)) return -EINVAL; - /* parse WME attributes if sta is WME capable */ - if ((rdev->wiphy.flags & WIPHY_FLAG_AP_UAPSD) && - (params.sta_flags_set & BIT(NL80211_STA_FLAG_WME)) && - info->attrs[NL80211_ATTR_STA_WME]) { - struct nlattr *tb[NL80211_STA_WME_MAX + 1]; - struct nlattr *nla; + switch (dev->ieee80211_ptr->iftype) { + case NL80211_IFTYPE_AP: + case NL80211_IFTYPE_AP_VLAN: + case NL80211_IFTYPE_P2P_GO: + /* parse WME attributes if sta is WME capable */ + if ((rdev->wiphy.flags & WIPHY_FLAG_AP_UAPSD) && + (params.sta_flags_set & BIT(NL80211_STA_FLAG_WME)) && + info->attrs[NL80211_ATTR_STA_WME]) { + struct nlattr *tb[NL80211_STA_WME_MAX + 1]; + struct nlattr *nla; + + nla = info->attrs[NL80211_ATTR_STA_WME]; + err = nla_parse_nested(tb, NL80211_STA_WME_MAX, nla, + nl80211_sta_wme_policy); + if (err) + return err; - nla = info->attrs[NL80211_ATTR_STA_WME]; - err = nla_parse_nested(tb, NL80211_STA_WME_MAX, nla, - nl80211_sta_wme_policy); - if (err) - return err; + if (tb[NL80211_STA_WME_UAPSD_QUEUES]) + params.uapsd_queues = + nla_get_u8(tb[NL80211_STA_WME_UAPSD_QUEUES]); + if (params.uapsd_queues & + ~IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK) + return -EINVAL; - if (tb[NL80211_STA_WME_UAPSD_QUEUES]) - params.uapsd_queues = - nla_get_u8(tb[NL80211_STA_WME_UAPSD_QUEUES]); - if (params.uapsd_queues & ~IEEE80211_WMM_IE_STA_QOSINFO_AC_MASK) - return -EINVAL; + if (tb[NL80211_STA_WME_MAX_SP]) + params.max_sp = + nla_get_u8(tb[NL80211_STA_WME_MAX_SP]); - if (tb[NL80211_STA_WME_MAX_SP]) - params.max_sp = - nla_get_u8(tb[NL80211_STA_WME_MAX_SP]); + if (params.max_sp & + ~IEEE80211_WMM_IE_STA_QOSINFO_SP_MASK) + return -EINVAL; - if (params.max_sp & ~IEEE80211_WMM_IE_STA_QOSINFO_SP_MASK) + params.sta_modify_mask |= STATION_PARAM_APPLY_UAPSD; + } + /* TDLS peers cannot be added */ + if (params.sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER)) return -EINVAL; + /* but don't bother the driver with it */ + params.sta_flags_mask &= ~BIT(NL80211_STA_FLAG_TDLS_PEER); - params.sta_modify_mask |= STATION_PARAM_APPLY_UAPSD; + /* must be last in here for error handling */ + params.vlan = get_vlan(info, rdev); + if (IS_ERR(params.vlan)) + return PTR_ERR(params.vlan); + break; + case NL80211_IFTYPE_MESH_POINT: + /* TDLS peers cannot be added */ + if (params.sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER)) + return -EINVAL; + break; + case NL80211_IFTYPE_STATION: + /* Only TDLS peers can be added */ + if (!(params.sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER))) + return -EINVAL; + /* Can only add if TDLS ... */ + if (!(rdev->wiphy.flags & WIPHY_FLAG_SUPPORTS_TDLS)) + return -EOPNOTSUPP; + /* ... with external setup is supported */ + if (!(rdev->wiphy.flags & WIPHY_FLAG_TDLS_EXTERNAL_SETUP)) + return -EOPNOTSUPP; + break; + default: + return -EOPNOTSUPP; } - if (dev->ieee80211_ptr->iftype != NL80211_IFTYPE_AP && - dev->ieee80211_ptr->iftype != NL80211_IFTYPE_AP_VLAN && - dev->ieee80211_ptr->iftype != NL80211_IFTYPE_MESH_POINT && - dev->ieee80211_ptr->iftype != NL80211_IFTYPE_P2P_GO && - dev->ieee80211_ptr->iftype != NL80211_IFTYPE_STATION) - return -EINVAL; - - /* - * Only managed stations can add TDLS peers, and only when the - * wiphy supports external TDLS setup. - */ - if (dev->ieee80211_ptr->iftype == NL80211_IFTYPE_STATION && - !((params.sta_flags_set & BIT(NL80211_STA_FLAG_TDLS_PEER)) && - (rdev->wiphy.flags & WIPHY_FLAG_SUPPORTS_TDLS) && - (rdev->wiphy.flags & WIPHY_FLAG_TDLS_EXTERNAL_SETUP))) - return -EINVAL; - - params.vlan = get_vlan(info, rdev); - if (IS_ERR(params.vlan)) - return PTR_ERR(params.vlan); - - /* validate settings */ - err = 0; - - if (!rdev->ops->add_station) { - err = -EOPNOTSUPP; - goto out; - } + /* be aware of params.vlan when changing code here */ err = rdev->ops->add_station(&rdev->wiphy, dev, mac_addr, ¶ms); - out: if (params.vlan) dev_put(params.vlan); return err; -- cgit v1.2.3-59-g8ed1b