forked from luck/tmp_suning_uos_patched
Merge branch 'for-upstream' of git://git.kernel.org/pub/scm/linux/kernel/git/bluetooth/bluetooth-next
Johan Hedberg says: ==================== pull request: bluetooth-next 2015-01-16 Here are some more bluetooth & ieee802154 patches intended for 3.20: - Refactoring & cleanups of ieee802154 & 6lowpan code - Various fixes to the btmrvl driver - Fixes for Bluetooth Low Energy Privacy feature handling - Added build-time sanity checks for sockaddr sizes - Fixes for Security Manager registration on LE-only controllers - Refactoring of broken inquiry mode handling to a generic quirk Please let me know if there are any issues pulling. Thanks. ==================== Signed-off-by: David S. Miller <davem@davemloft.net>
This commit is contained in:
commit
e445dd5f67
|
@ -28,9 +28,9 @@
|
|||
#define BTM_UPLD_SIZE 2312
|
||||
|
||||
/* Time to wait until Host Sleep state change in millisecond */
|
||||
#define WAIT_UNTIL_HS_STATE_CHANGED 5000
|
||||
#define WAIT_UNTIL_HS_STATE_CHANGED msecs_to_jiffies(5000)
|
||||
/* Time to wait for command response in millisecond */
|
||||
#define WAIT_UNTIL_CMD_RESP 5000
|
||||
#define WAIT_UNTIL_CMD_RESP msecs_to_jiffies(5000)
|
||||
|
||||
enum rdwr_status {
|
||||
RDWR_STATUS_SUCCESS = 0,
|
||||
|
@ -104,6 +104,7 @@ struct btmrvl_private {
|
|||
#ifdef CONFIG_DEBUG_FS
|
||||
void *debugfs_data;
|
||||
#endif
|
||||
bool surprise_removed;
|
||||
};
|
||||
|
||||
#define MRVL_VENDOR_PKT 0xFE
|
||||
|
|
|
@ -178,6 +178,11 @@ static int btmrvl_send_sync_cmd(struct btmrvl_private *priv, u16 opcode,
|
|||
struct sk_buff *skb;
|
||||
struct hci_command_hdr *hdr;
|
||||
|
||||
if (priv->surprise_removed) {
|
||||
BT_ERR("Card is removed");
|
||||
return -EFAULT;
|
||||
}
|
||||
|
||||
skb = bt_skb_alloc(HCI_COMMAND_HDR_SIZE + len, GFP_ATOMIC);
|
||||
if (skb == NULL) {
|
||||
BT_ERR("No free skb");
|
||||
|
@ -202,10 +207,14 @@ static int btmrvl_send_sync_cmd(struct btmrvl_private *priv, u16 opcode,
|
|||
wake_up_interruptible(&priv->main_thread.wait_q);
|
||||
|
||||
if (!wait_event_interruptible_timeout(priv->adapter->cmd_wait_q,
|
||||
priv->adapter->cmd_complete,
|
||||
msecs_to_jiffies(WAIT_UNTIL_CMD_RESP)))
|
||||
priv->adapter->cmd_complete ||
|
||||
priv->surprise_removed,
|
||||
WAIT_UNTIL_CMD_RESP))
|
||||
return -ETIMEDOUT;
|
||||
|
||||
if (priv->surprise_removed)
|
||||
return -EFAULT;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
@ -287,9 +296,10 @@ int btmrvl_enable_hs(struct btmrvl_private *priv)
|
|||
}
|
||||
|
||||
ret = wait_event_interruptible_timeout(adapter->event_hs_wait_q,
|
||||
adapter->hs_state,
|
||||
msecs_to_jiffies(WAIT_UNTIL_HS_STATE_CHANGED));
|
||||
if (ret < 0) {
|
||||
adapter->hs_state ||
|
||||
priv->surprise_removed,
|
||||
WAIT_UNTIL_HS_STATE_CHANGED);
|
||||
if (ret < 0 || priv->surprise_removed) {
|
||||
BT_ERR("event_hs_wait_q terminated (%d): %d,%d,%d",
|
||||
ret, adapter->hs_state, adapter->ps_state,
|
||||
adapter->wakeup_tries);
|
||||
|
@ -538,8 +548,11 @@ static int btmrvl_check_device_tree(struct btmrvl_private *priv)
|
|||
static int btmrvl_setup(struct hci_dev *hdev)
|
||||
{
|
||||
struct btmrvl_private *priv = hci_get_drvdata(hdev);
|
||||
int ret;
|
||||
|
||||
btmrvl_send_module_cfg_cmd(priv, MODULE_BRINGUP_REQ);
|
||||
ret = btmrvl_send_module_cfg_cmd(priv, MODULE_BRINGUP_REQ);
|
||||
if (ret)
|
||||
return ret;
|
||||
|
||||
priv->btmrvl_dev.gpio_gap = 0xffff;
|
||||
|
||||
|
@ -597,7 +610,7 @@ static int btmrvl_service_main_thread(void *data)
|
|||
add_wait_queue(&thread->wait_q, &wait);
|
||||
|
||||
set_current_state(TASK_INTERRUPTIBLE);
|
||||
if (kthread_should_stop()) {
|
||||
if (kthread_should_stop() || priv->surprise_removed) {
|
||||
BT_DBG("main_thread: break from main thread");
|
||||
break;
|
||||
}
|
||||
|
@ -616,6 +629,11 @@ static int btmrvl_service_main_thread(void *data)
|
|||
|
||||
BT_DBG("main_thread woke up");
|
||||
|
||||
if (kthread_should_stop() || priv->surprise_removed) {
|
||||
BT_DBG("main_thread: break from main thread");
|
||||
break;
|
||||
}
|
||||
|
||||
spin_lock_irqsave(&priv->driver_lock, flags);
|
||||
if (adapter->int_count) {
|
||||
adapter->int_count = 0;
|
||||
|
|
|
@ -573,7 +573,7 @@ static int btmrvl_sdio_download_fw_w_helper(struct btmrvl_sdio_card *card)
|
|||
offset += txlen;
|
||||
} while (true);
|
||||
|
||||
BT_DBG("FW download over, size %d bytes", offset);
|
||||
BT_INFO("FW download over, size %d bytes", offset);
|
||||
|
||||
ret = 0;
|
||||
|
||||
|
@ -798,6 +798,9 @@ static void btmrvl_sdio_interrupt(struct sdio_func *func)
|
|||
|
||||
priv = card->priv;
|
||||
|
||||
if (priv->surprise_removed)
|
||||
return;
|
||||
|
||||
if (card->reg->int_read_to_clear)
|
||||
ret = btmrvl_sdio_read_to_clear(card, &ireg);
|
||||
else
|
||||
|
@ -1466,6 +1469,7 @@ static void btmrvl_sdio_remove(struct sdio_func *func)
|
|||
btmrvl_sdio_disable_host_int(card);
|
||||
}
|
||||
BT_DBG("unregester dev");
|
||||
card->priv->surprise_removed = true;
|
||||
btmrvl_sdio_unregister_dev(card);
|
||||
btmrvl_remove_card(card->priv);
|
||||
}
|
||||
|
|
|
@ -49,7 +49,7 @@ static struct usb_driver btusb_driver;
|
|||
#define BTUSB_INTEL_BOOT 0x200
|
||||
#define BTUSB_BCM_PATCHRAM 0x400
|
||||
#define BTUSB_MARVELL 0x800
|
||||
#define BTUSB_AVM 0x1000
|
||||
#define BTUSB_SWAVE 0x1000
|
||||
|
||||
static const struct usb_device_id btusb_table[] = {
|
||||
/* Generic Bluetooth USB device */
|
||||
|
@ -86,7 +86,7 @@ static const struct usb_device_id btusb_table[] = {
|
|||
{ USB_DEVICE(0x05ac, 0x8281) },
|
||||
|
||||
/* AVM BlueFRITZ! USB v2.0 */
|
||||
{ USB_DEVICE(0x057c, 0x3800), .driver_info = BTUSB_AVM },
|
||||
{ USB_DEVICE(0x057c, 0x3800), .driver_info = BTUSB_SWAVE },
|
||||
|
||||
/* Bluetooth Ultraport Module from IBM */
|
||||
{ USB_DEVICE(0x04bf, 0x030a) },
|
||||
|
@ -238,6 +238,9 @@ static const struct usb_device_id blacklist_table[] = {
|
|||
/* CONWISE Technology based adapters with buggy SCO support */
|
||||
{ USB_DEVICE(0x0e5e, 0x6622), .driver_info = BTUSB_BROKEN_ISOC },
|
||||
|
||||
/* Roper Class 1 Bluetooth Dongle (Silicon Wave based) */
|
||||
{ USB_DEVICE(0x1300, 0x0001), .driver_info = BTUSB_SWAVE },
|
||||
|
||||
/* Digianswer devices */
|
||||
{ USB_DEVICE(0x08fd, 0x0001), .driver_info = BTUSB_DIGIANSWER },
|
||||
{ USB_DEVICE(0x08fd, 0x0002), .driver_info = BTUSB_IGNORE },
|
||||
|
@ -306,6 +309,7 @@ struct btusb_data {
|
|||
int isoc_altsetting;
|
||||
int suspend_count;
|
||||
|
||||
int (*recv_event)(struct hci_dev *hdev, struct sk_buff *skb);
|
||||
int (*recv_bulk)(struct btusb_data *data, void *buffer, int count);
|
||||
};
|
||||
|
||||
|
@ -371,7 +375,7 @@ static int btusb_recv_intr(struct btusb_data *data, void *buffer, int count)
|
|||
|
||||
if (bt_cb(skb)->expect == 0) {
|
||||
/* Complete frame */
|
||||
hci_recv_frame(data->hdev, skb);
|
||||
data->recv_event(data->hdev, skb);
|
||||
skb = NULL;
|
||||
}
|
||||
}
|
||||
|
@ -2045,6 +2049,7 @@ static int btusb_probe(struct usb_interface *intf,
|
|||
init_usb_anchor(&data->isoc_anchor);
|
||||
spin_lock_init(&data->rxlock);
|
||||
|
||||
data->recv_event = hci_recv_frame;
|
||||
data->recv_bulk = btusb_recv_bulk;
|
||||
|
||||
hdev = hci_alloc_dev();
|
||||
|
@ -2081,8 +2086,10 @@ static int btusb_probe(struct usb_interface *intf,
|
|||
if (id->driver_info & BTUSB_MARVELL)
|
||||
hdev->set_bdaddr = btusb_set_bdaddr_marvell;
|
||||
|
||||
if (id->driver_info & BTUSB_AVM)
|
||||
if (id->driver_info & BTUSB_SWAVE) {
|
||||
set_bit(HCI_QUIRK_FIXUP_INQUIRY_MODE, &hdev->quirks);
|
||||
set_bit(HCI_QUIRK_BROKEN_LOCAL_COMMANDS, &hdev->quirks);
|
||||
}
|
||||
|
||||
if (id->driver_info & BTUSB_INTEL_BOOT)
|
||||
set_bit(HCI_QUIRK_RAW_DEVICE, &hdev->quirks);
|
||||
|
|
|
@ -273,7 +273,7 @@ struct l2cap_ctrl {
|
|||
|
||||
struct hci_dev;
|
||||
|
||||
typedef void (*hci_req_complete_t)(struct hci_dev *hdev, u8 status);
|
||||
typedef void (*hci_req_complete_t)(struct hci_dev *hdev, u8 status, u16 opcode);
|
||||
|
||||
struct hci_req_ctrl {
|
||||
bool start;
|
||||
|
|
|
@ -102,6 +102,18 @@ enum {
|
|||
*/
|
||||
HCI_QUIRK_FIXUP_BUFFER_SIZE,
|
||||
|
||||
/* When this quirk is set, then a controller that does not
|
||||
* indicate support for Inquiry Result with RSSI is assumed to
|
||||
* support it anyway. Some early Bluetooth 1.2 controllers had
|
||||
* wrongly configured local features that will require forcing
|
||||
* them to enable this mode. Getting RSSI information with the
|
||||
* inquiry responses is preferred since it allows for a better
|
||||
* user expierence.
|
||||
*
|
||||
* This quirk must be set before hci_register_dev is called.
|
||||
*/
|
||||
HCI_QUIRK_FIXUP_INQUIRY_MODE,
|
||||
|
||||
/* When this quirk is set, then the HCI Read Local Supported
|
||||
* Commands command is not supported. In general Bluetooth 1.2
|
||||
* and later controllers should support this command. However
|
||||
|
@ -172,8 +184,7 @@ enum {
|
|||
*/
|
||||
enum {
|
||||
HCI_DUT_MODE,
|
||||
HCI_FORCE_SC,
|
||||
HCI_FORCE_LESC,
|
||||
HCI_FORCE_BREDR_SMP,
|
||||
HCI_FORCE_STATIC_ADDR,
|
||||
};
|
||||
|
||||
|
@ -844,11 +855,26 @@ struct hci_cp_set_event_flt {
|
|||
#define HCI_CONN_SETUP_AUTO_OFF 0x01
|
||||
#define HCI_CONN_SETUP_AUTO_ON 0x02
|
||||
|
||||
#define HCI_OP_READ_STORED_LINK_KEY 0x0c0d
|
||||
struct hci_cp_read_stored_link_key {
|
||||
bdaddr_t bdaddr;
|
||||
__u8 read_all;
|
||||
} __packed;
|
||||
struct hci_rp_read_stored_link_key {
|
||||
__u8 status;
|
||||
__u8 max_keys;
|
||||
__u8 num_keys;
|
||||
} __packed;
|
||||
|
||||
#define HCI_OP_DELETE_STORED_LINK_KEY 0x0c12
|
||||
struct hci_cp_delete_stored_link_key {
|
||||
bdaddr_t bdaddr;
|
||||
__u8 delete_all;
|
||||
} __packed;
|
||||
struct hci_rp_delete_stored_link_key {
|
||||
__u8 status;
|
||||
__u8 num_keys;
|
||||
} __packed;
|
||||
|
||||
#define HCI_MAX_NAME_LENGTH 248
|
||||
|
||||
|
|
|
@ -205,6 +205,8 @@ struct hci_dev {
|
|||
__u16 lmp_subver;
|
||||
__u16 voice_setting;
|
||||
__u8 num_iac;
|
||||
__u8 stored_max_keys;
|
||||
__u8 stored_num_keys;
|
||||
__u8 io_capability;
|
||||
__s8 inq_tx_power;
|
||||
__u16 page_scan_interval;
|
||||
|
@ -779,7 +781,6 @@ int hci_conn_check_link_mode(struct hci_conn *conn);
|
|||
int hci_conn_check_secure(struct hci_conn *conn, __u8 sec_level);
|
||||
int hci_conn_security(struct hci_conn *conn, __u8 sec_level, __u8 auth_type,
|
||||
bool initiator);
|
||||
int hci_conn_change_link_key(struct hci_conn *conn);
|
||||
int hci_conn_switch_role(struct hci_conn *conn, __u8 role);
|
||||
|
||||
void hci_conn_enter_active_mode(struct hci_conn *conn, __u8 force_active);
|
||||
|
@ -1017,8 +1018,7 @@ void hci_conn_del_sysfs(struct hci_conn *conn);
|
|||
|
||||
#define hdev_is_powered(hdev) (test_bit(HCI_UP, &hdev->flags) && \
|
||||
!test_bit(HCI_AUTO_OFF, &hdev->dev_flags))
|
||||
#define bredr_sc_enabled(dev) ((lmp_sc_capable(dev) || \
|
||||
test_bit(HCI_FORCE_SC, &(dev)->dbg_flags)) && \
|
||||
#define bredr_sc_enabled(dev) (lmp_sc_capable(dev) && \
|
||||
test_bit(HCI_SC_ENABLED, &(dev)->dev_flags))
|
||||
|
||||
/* ----- HCI protocols ----- */
|
||||
|
|
|
@ -31,7 +31,7 @@
|
|||
|
||||
#define VERSION "0.1"
|
||||
|
||||
static struct dentry *lowpan_psm_debugfs;
|
||||
static struct dentry *lowpan_enable_debugfs;
|
||||
static struct dentry *lowpan_control_debugfs;
|
||||
|
||||
#define IFACE_NAME_TEMPLATE "bt%d"
|
||||
|
@ -55,11 +55,7 @@ struct skb_cb {
|
|||
static LIST_HEAD(bt_6lowpan_devices);
|
||||
static DEFINE_SPINLOCK(devices_lock);
|
||||
|
||||
/* If psm is set to 0 (default value), then 6lowpan is disabled.
|
||||
* Other values are used to indicate a Protocol Service Multiplexer
|
||||
* value for 6lowpan.
|
||||
*/
|
||||
static u16 psm_6lowpan;
|
||||
static bool enable_6lowpan;
|
||||
|
||||
/* We are listening incoming connections via this channel
|
||||
*/
|
||||
|
@ -761,7 +757,7 @@ static bool is_bt_6lowpan(struct hci_conn *hcon)
|
|||
if (hcon->type != LE_LINK)
|
||||
return false;
|
||||
|
||||
if (!psm_6lowpan)
|
||||
if (!enable_6lowpan)
|
||||
return false;
|
||||
|
||||
return true;
|
||||
|
@ -1085,7 +1081,7 @@ static int bt_6lowpan_connect(bdaddr_t *addr, u8 dst_type)
|
|||
if (!pchan)
|
||||
return -EINVAL;
|
||||
|
||||
err = l2cap_chan_connect(pchan, cpu_to_le16(psm_6lowpan), 0,
|
||||
err = l2cap_chan_connect(pchan, cpu_to_le16(L2CAP_PSM_IPSP), 0,
|
||||
addr, dst_type);
|
||||
|
||||
BT_DBG("chan %p err %d", pchan, err);
|
||||
|
@ -1118,7 +1114,7 @@ static struct l2cap_chan *bt_6lowpan_listen(void)
|
|||
struct l2cap_chan *pchan;
|
||||
int err;
|
||||
|
||||
if (psm_6lowpan == 0)
|
||||
if (!enable_6lowpan)
|
||||
return NULL;
|
||||
|
||||
pchan = chan_get();
|
||||
|
@ -1130,10 +1126,9 @@ static struct l2cap_chan *bt_6lowpan_listen(void)
|
|||
|
||||
atomic_set(&pchan->nesting, L2CAP_NESTING_PARENT);
|
||||
|
||||
BT_DBG("psm 0x%04x chan %p src type %d", psm_6lowpan, pchan,
|
||||
pchan->src_type);
|
||||
BT_DBG("chan %p src type %d", pchan, pchan->src_type);
|
||||
|
||||
err = l2cap_add_psm(pchan, addr, cpu_to_le16(psm_6lowpan));
|
||||
err = l2cap_add_psm(pchan, addr, cpu_to_le16(L2CAP_PSM_IPSP));
|
||||
if (err) {
|
||||
l2cap_chan_put(pchan);
|
||||
BT_ERR("psm cannot be added err %d", err);
|
||||
|
@ -1219,22 +1214,23 @@ static void disconnect_all_peers(void)
|
|||
spin_unlock(&devices_lock);
|
||||
}
|
||||
|
||||
struct set_psm {
|
||||
struct set_enable {
|
||||
struct work_struct work;
|
||||
u16 psm;
|
||||
bool flag;
|
||||
};
|
||||
|
||||
static void do_psm_set(struct work_struct *work)
|
||||
static void do_enable_set(struct work_struct *work)
|
||||
{
|
||||
struct set_psm *set_psm = container_of(work, struct set_psm, work);
|
||||
struct set_enable *set_enable = container_of(work,
|
||||
struct set_enable, work);
|
||||
|
||||
if (set_psm->psm == 0 || psm_6lowpan != set_psm->psm)
|
||||
if (!set_enable->flag || enable_6lowpan != set_enable->flag)
|
||||
/* Disconnect existing connections if 6lowpan is
|
||||
* disabled (psm = 0), or if psm changes.
|
||||
* disabled
|
||||
*/
|
||||
disconnect_all_peers();
|
||||
|
||||
psm_6lowpan = set_psm->psm;
|
||||
enable_6lowpan = set_enable->flag;
|
||||
|
||||
if (listen_chan) {
|
||||
l2cap_chan_close(listen_chan, 0);
|
||||
|
@ -1243,33 +1239,33 @@ static void do_psm_set(struct work_struct *work)
|
|||
|
||||
listen_chan = bt_6lowpan_listen();
|
||||
|
||||
kfree(set_psm);
|
||||
kfree(set_enable);
|
||||
}
|
||||
|
||||
static int lowpan_psm_set(void *data, u64 val)
|
||||
static int lowpan_enable_set(void *data, u64 val)
|
||||
{
|
||||
struct set_psm *set_psm;
|
||||
struct set_enable *set_enable;
|
||||
|
||||
set_psm = kzalloc(sizeof(*set_psm), GFP_KERNEL);
|
||||
if (!set_psm)
|
||||
set_enable = kzalloc(sizeof(*set_enable), GFP_KERNEL);
|
||||
if (!set_enable)
|
||||
return -ENOMEM;
|
||||
|
||||
set_psm->psm = val;
|
||||
INIT_WORK(&set_psm->work, do_psm_set);
|
||||
set_enable->flag = !!val;
|
||||
INIT_WORK(&set_enable->work, do_enable_set);
|
||||
|
||||
schedule_work(&set_psm->work);
|
||||
schedule_work(&set_enable->work);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lowpan_psm_get(void *data, u64 *val)
|
||||
static int lowpan_enable_get(void *data, u64 *val)
|
||||
{
|
||||
*val = psm_6lowpan;
|
||||
*val = enable_6lowpan;
|
||||
return 0;
|
||||
}
|
||||
|
||||
DEFINE_SIMPLE_ATTRIBUTE(lowpan_psm_fops, lowpan_psm_get,
|
||||
lowpan_psm_set, "%llu\n");
|
||||
DEFINE_SIMPLE_ATTRIBUTE(lowpan_enable_fops, lowpan_enable_get,
|
||||
lowpan_enable_set, "%llu\n");
|
||||
|
||||
static ssize_t lowpan_control_write(struct file *fp,
|
||||
const char __user *user_buffer,
|
||||
|
@ -1439,9 +1435,9 @@ static struct notifier_block bt_6lowpan_dev_notifier = {
|
|||
|
||||
static int __init bt_6lowpan_init(void)
|
||||
{
|
||||
lowpan_psm_debugfs = debugfs_create_file("6lowpan_psm", 0644,
|
||||
bt_debugfs, NULL,
|
||||
&lowpan_psm_fops);
|
||||
lowpan_enable_debugfs = debugfs_create_file("6lowpan_enable", 0644,
|
||||
bt_debugfs, NULL,
|
||||
&lowpan_enable_fops);
|
||||
lowpan_control_debugfs = debugfs_create_file("6lowpan_control", 0644,
|
||||
bt_debugfs, NULL,
|
||||
&lowpan_control_fops);
|
||||
|
@ -1451,7 +1447,7 @@ static int __init bt_6lowpan_init(void)
|
|||
|
||||
static void __exit bt_6lowpan_exit(void)
|
||||
{
|
||||
debugfs_remove(lowpan_psm_debugfs);
|
||||
debugfs_remove(lowpan_enable_debugfs);
|
||||
debugfs_remove(lowpan_control_debugfs);
|
||||
|
||||
if (listen_chan) {
|
||||
|
|
|
@ -253,8 +253,6 @@ static void cmtp_recv_interopmsg(struct cmtp_session *session, struct sk_buff *s
|
|||
if (skb->len < CAPI_MSG_BASELEN + 15)
|
||||
break;
|
||||
|
||||
controller = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 10);
|
||||
|
||||
if (!info && ctrl) {
|
||||
int len = min_t(uint, CAPI_MANUFACTURER_LEN,
|
||||
skb->data[CAPI_MSG_BASELEN + 14]);
|
||||
|
@ -270,8 +268,6 @@ static void cmtp_recv_interopmsg(struct cmtp_session *session, struct sk_buff *s
|
|||
if (skb->len < CAPI_MSG_BASELEN + 32)
|
||||
break;
|
||||
|
||||
controller = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 12);
|
||||
|
||||
if (!info && ctrl) {
|
||||
ctrl->version.majorversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 16);
|
||||
ctrl->version.minorversion = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 20);
|
||||
|
@ -285,8 +281,6 @@ static void cmtp_recv_interopmsg(struct cmtp_session *session, struct sk_buff *s
|
|||
if (skb->len < CAPI_MSG_BASELEN + 17)
|
||||
break;
|
||||
|
||||
controller = CAPIMSG_U32(skb->data, CAPI_MSG_BASELEN + 12);
|
||||
|
||||
if (!info && ctrl) {
|
||||
int len = min_t(uint, CAPI_SERIAL_LEN,
|
||||
skb->data[CAPI_MSG_BASELEN + 16]);
|
||||
|
|
|
@ -633,7 +633,7 @@ void hci_le_conn_failed(struct hci_conn *conn, u8 status)
|
|||
mgmt_reenable_advertising(hdev);
|
||||
}
|
||||
|
||||
static void create_le_conn_complete(struct hci_dev *hdev, u8 status)
|
||||
static void create_le_conn_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct hci_conn *conn;
|
||||
|
||||
|
@ -1084,21 +1084,6 @@ int hci_conn_check_secure(struct hci_conn *conn, __u8 sec_level)
|
|||
}
|
||||
EXPORT_SYMBOL(hci_conn_check_secure);
|
||||
|
||||
/* Change link key */
|
||||
int hci_conn_change_link_key(struct hci_conn *conn)
|
||||
{
|
||||
BT_DBG("hcon %p", conn);
|
||||
|
||||
if (!test_and_set_bit(HCI_CONN_AUTH_PEND, &conn->flags)) {
|
||||
struct hci_cp_change_conn_link_key cp;
|
||||
cp.handle = cpu_to_le16(conn->handle);
|
||||
hci_send_cmd(conn->hdev, HCI_OP_CHANGE_CONN_LINK_KEY,
|
||||
sizeof(cp), &cp);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Switch role */
|
||||
int hci_conn_switch_role(struct hci_conn *conn, __u8 role)
|
||||
{
|
||||
|
|
|
@ -141,7 +141,7 @@ static const struct file_operations dut_mode_fops = {
|
|||
|
||||
/* ---- HCI requests ---- */
|
||||
|
||||
static void hci_req_sync_complete(struct hci_dev *hdev, u8 result)
|
||||
static void hci_req_sync_complete(struct hci_dev *hdev, u8 result, u16 opcode)
|
||||
{
|
||||
BT_DBG("%s result 0x%2.2x", hdev->name, result);
|
||||
|
||||
|
@ -497,43 +497,6 @@ static void le_setup(struct hci_request *req)
|
|||
set_bit(HCI_LE_ENABLED, &hdev->dev_flags);
|
||||
}
|
||||
|
||||
static u8 hci_get_inquiry_mode(struct hci_dev *hdev)
|
||||
{
|
||||
if (lmp_ext_inq_capable(hdev))
|
||||
return 0x02;
|
||||
|
||||
if (lmp_inq_rssi_capable(hdev))
|
||||
return 0x01;
|
||||
|
||||
if (hdev->manufacturer == 11 && hdev->hci_rev == 0x00 &&
|
||||
hdev->lmp_subver == 0x0757)
|
||||
return 0x01;
|
||||
|
||||
if (hdev->manufacturer == 15) {
|
||||
if (hdev->hci_rev == 0x03 && hdev->lmp_subver == 0x6963)
|
||||
return 0x01;
|
||||
if (hdev->hci_rev == 0x09 && hdev->lmp_subver == 0x6963)
|
||||
return 0x01;
|
||||
if (hdev->hci_rev == 0x00 && hdev->lmp_subver == 0x6965)
|
||||
return 0x01;
|
||||
}
|
||||
|
||||
if (hdev->manufacturer == 31 && hdev->hci_rev == 0x2005 &&
|
||||
hdev->lmp_subver == 0x1805)
|
||||
return 0x01;
|
||||
|
||||
return 0x00;
|
||||
}
|
||||
|
||||
static void hci_setup_inquiry_mode(struct hci_request *req)
|
||||
{
|
||||
u8 mode;
|
||||
|
||||
mode = hci_get_inquiry_mode(req->hdev);
|
||||
|
||||
hci_req_add(req, HCI_OP_WRITE_INQUIRY_MODE, 1, &mode);
|
||||
}
|
||||
|
||||
static void hci_setup_event_mask(struct hci_request *req)
|
||||
{
|
||||
struct hci_dev *hdev = req->hdev;
|
||||
|
@ -658,8 +621,18 @@ static void hci_init2_req(struct hci_request *req, unsigned long opt)
|
|||
}
|
||||
}
|
||||
|
||||
if (lmp_inq_rssi_capable(hdev))
|
||||
hci_setup_inquiry_mode(req);
|
||||
if (lmp_inq_rssi_capable(hdev) ||
|
||||
test_bit(HCI_QUIRK_FIXUP_INQUIRY_MODE, &hdev->quirks)) {
|
||||
u8 mode;
|
||||
|
||||
/* If Extended Inquiry Result events are supported, then
|
||||
* they are clearly preferred over Inquiry Result with RSSI
|
||||
* events.
|
||||
*/
|
||||
mode = lmp_ext_inq_capable(hdev) ? 0x02 : 0x01;
|
||||
|
||||
hci_req_add(req, HCI_OP_WRITE_INQUIRY_MODE, 1, &mode);
|
||||
}
|
||||
|
||||
if (lmp_inq_tx_pwr_capable(hdev))
|
||||
hci_req_add(req, HCI_OP_READ_INQ_RSP_TX_POWER, 0, NULL);
|
||||
|
@ -758,27 +731,12 @@ static void hci_init3_req(struct hci_request *req, unsigned long opt)
|
|||
|
||||
hci_setup_event_mask(req);
|
||||
|
||||
/* Some Broadcom based Bluetooth controllers do not support the
|
||||
* Delete Stored Link Key command. They are clearly indicating its
|
||||
* absence in the bit mask of supported commands.
|
||||
*
|
||||
* Check the supported commands and only if the the command is marked
|
||||
* as supported send it. If not supported assume that the controller
|
||||
* does not have actual support for stored link keys which makes this
|
||||
* command redundant anyway.
|
||||
*
|
||||
* Some controllers indicate that they support handling deleting
|
||||
* stored link keys, but they don't. The quirk lets a driver
|
||||
* just disable this command.
|
||||
*/
|
||||
if (hdev->commands[6] & 0x80 &&
|
||||
!test_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY, &hdev->quirks)) {
|
||||
struct hci_cp_delete_stored_link_key cp;
|
||||
if (hdev->commands[6] & 0x20) {
|
||||
struct hci_cp_read_stored_link_key cp;
|
||||
|
||||
bacpy(&cp.bdaddr, BDADDR_ANY);
|
||||
cp.delete_all = 0x01;
|
||||
hci_req_add(req, HCI_OP_DELETE_STORED_LINK_KEY,
|
||||
sizeof(cp), &cp);
|
||||
cp.read_all = 0x01;
|
||||
hci_req_add(req, HCI_OP_READ_STORED_LINK_KEY, sizeof(cp), &cp);
|
||||
}
|
||||
|
||||
if (hdev->commands[5] & 0x10)
|
||||
|
@ -872,6 +830,29 @@ static void hci_init4_req(struct hci_request *req, unsigned long opt)
|
|||
{
|
||||
struct hci_dev *hdev = req->hdev;
|
||||
|
||||
/* Some Broadcom based Bluetooth controllers do not support the
|
||||
* Delete Stored Link Key command. They are clearly indicating its
|
||||
* absence in the bit mask of supported commands.
|
||||
*
|
||||
* Check the supported commands and only if the the command is marked
|
||||
* as supported send it. If not supported assume that the controller
|
||||
* does not have actual support for stored link keys which makes this
|
||||
* command redundant anyway.
|
||||
*
|
||||
* Some controllers indicate that they support handling deleting
|
||||
* stored link keys, but they don't. The quirk lets a driver
|
||||
* just disable this command.
|
||||
*/
|
||||
if (hdev->commands[6] & 0x80 &&
|
||||
!test_bit(HCI_QUIRK_BROKEN_STORED_LINK_KEY, &hdev->quirks)) {
|
||||
struct hci_cp_delete_stored_link_key cp;
|
||||
|
||||
bacpy(&cp.bdaddr, BDADDR_ANY);
|
||||
cp.delete_all = 0x01;
|
||||
hci_req_add(req, HCI_OP_DELETE_STORED_LINK_KEY,
|
||||
sizeof(cp), &cp);
|
||||
}
|
||||
|
||||
/* Set event mask page 2 if the HCI command for it is supported */
|
||||
if (hdev->commands[22] & 0x04)
|
||||
hci_set_event_mask_page_2(req);
|
||||
|
@ -931,10 +912,20 @@ static int __hci_init(struct hci_dev *hdev)
|
|||
if (err < 0)
|
||||
return err;
|
||||
|
||||
/* Only create debugfs entries during the initial setup
|
||||
* phase and not every time the controller gets powered on.
|
||||
/* This function is only called when the controller is actually in
|
||||
* configured state. When the controller is marked as unconfigured,
|
||||
* this initialization procedure is not run.
|
||||
*
|
||||
* It means that it is possible that a controller runs through its
|
||||
* setup phase and then discovers missing settings. If that is the
|
||||
* case, then this function will not be called. It then will only
|
||||
* be called during the config phase.
|
||||
*
|
||||
* So only when in setup phase or config phase, create the debugfs
|
||||
* entries and register the SMP channels.
|
||||
*/
|
||||
if (!test_bit(HCI_SETUP, &hdev->dev_flags))
|
||||
if (!test_bit(HCI_SETUP, &hdev->dev_flags) &&
|
||||
!test_bit(HCI_CONFIG, &hdev->dev_flags))
|
||||
return 0;
|
||||
|
||||
hci_debugfs_create_common(hdev);
|
||||
|
@ -942,10 +933,8 @@ static int __hci_init(struct hci_dev *hdev)
|
|||
if (lmp_bredr_capable(hdev))
|
||||
hci_debugfs_create_bredr(hdev);
|
||||
|
||||
if (lmp_le_capable(hdev)) {
|
||||
if (lmp_le_capable(hdev))
|
||||
hci_debugfs_create_le(hdev);
|
||||
smp_register(hdev);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
@ -2142,6 +2131,8 @@ static void hci_power_off(struct work_struct *work)
|
|||
BT_DBG("%s", hdev->name);
|
||||
|
||||
hci_dev_do_close(hdev);
|
||||
|
||||
smp_unregister(hdev);
|
||||
}
|
||||
|
||||
static void hci_discov_off(struct work_struct *work)
|
||||
|
@ -2771,7 +2762,7 @@ void hci_conn_params_clear_all(struct hci_dev *hdev)
|
|||
BT_DBG("All LE connection parameters were removed");
|
||||
}
|
||||
|
||||
static void inquiry_complete(struct hci_dev *hdev, u8 status)
|
||||
static void inquiry_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
if (status) {
|
||||
BT_ERR("Failed to start inquiry: status %d", status);
|
||||
|
@ -2783,7 +2774,8 @@ static void inquiry_complete(struct hci_dev *hdev, u8 status)
|
|||
}
|
||||
}
|
||||
|
||||
static void le_scan_disable_work_complete(struct hci_dev *hdev, u8 status)
|
||||
static void le_scan_disable_work_complete(struct hci_dev *hdev, u8 status,
|
||||
u16 opcode)
|
||||
{
|
||||
/* General inquiry access code (GIAC) */
|
||||
u8 lap[3] = { 0x33, 0x8b, 0x9e };
|
||||
|
@ -4176,7 +4168,7 @@ void hci_req_cmd_complete(struct hci_dev *hdev, u16 opcode, u8 status)
|
|||
|
||||
call_complete:
|
||||
if (req_complete)
|
||||
req_complete(hdev, status);
|
||||
req_complete(hdev, status, status ? opcode : HCI_OP_NOP);
|
||||
}
|
||||
|
||||
static void hci_rx_work(struct work_struct *work)
|
||||
|
|
|
@ -212,6 +212,24 @@ static int conn_info_max_age_get(void *data, u64 *val)
|
|||
DEFINE_SIMPLE_ATTRIBUTE(conn_info_max_age_fops, conn_info_max_age_get,
|
||||
conn_info_max_age_set, "%llu\n");
|
||||
|
||||
static ssize_t sc_only_mode_read(struct file *file, char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[3];
|
||||
|
||||
buf[0] = test_bit(HCI_SC_ONLY, &hdev->dev_flags) ? 'Y': 'N';
|
||||
buf[1] = '\n';
|
||||
buf[2] = '\0';
|
||||
return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
|
||||
}
|
||||
|
||||
static const struct file_operations sc_only_mode_fops = {
|
||||
.open = simple_open,
|
||||
.read = sc_only_mode_read,
|
||||
.llseek = default_llseek,
|
||||
};
|
||||
|
||||
void hci_debugfs_create_common(struct hci_dev *hdev)
|
||||
{
|
||||
debugfs_create_file("features", 0444, hdev->debugfs, hdev,
|
||||
|
@ -230,6 +248,10 @@ void hci_debugfs_create_common(struct hci_dev *hdev)
|
|||
&conn_info_min_age_fops);
|
||||
debugfs_create_file("conn_info_max_age", 0644, hdev->debugfs, hdev,
|
||||
&conn_info_max_age_fops);
|
||||
|
||||
if (lmp_sc_capable(hdev) || lmp_le_capable(hdev))
|
||||
debugfs_create_file("sc_only_mode", 0444, hdev->debugfs,
|
||||
hdev, &sc_only_mode_fops);
|
||||
}
|
||||
|
||||
static int inquiry_cache_show(struct seq_file *f, void *p)
|
||||
|
@ -357,114 +379,6 @@ static int auto_accept_delay_get(void *data, u64 *val)
|
|||
DEFINE_SIMPLE_ATTRIBUTE(auto_accept_delay_fops, auto_accept_delay_get,
|
||||
auto_accept_delay_set, "%llu\n");
|
||||
|
||||
static ssize_t sc_only_mode_read(struct file *file, char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[3];
|
||||
|
||||
buf[0] = test_bit(HCI_SC_ONLY, &hdev->dev_flags) ? 'Y': 'N';
|
||||
buf[1] = '\n';
|
||||
buf[2] = '\0';
|
||||
return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
|
||||
}
|
||||
|
||||
static const struct file_operations sc_only_mode_fops = {
|
||||
.open = simple_open,
|
||||
.read = sc_only_mode_read,
|
||||
.llseek = default_llseek,
|
||||
};
|
||||
|
||||
static ssize_t force_sc_support_read(struct file *file, char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[3];
|
||||
|
||||
buf[0] = test_bit(HCI_FORCE_SC, &hdev->dbg_flags) ? 'Y': 'N';
|
||||
buf[1] = '\n';
|
||||
buf[2] = '\0';
|
||||
return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
|
||||
}
|
||||
|
||||
static ssize_t force_sc_support_write(struct file *file,
|
||||
const char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[32];
|
||||
size_t buf_size = min(count, (sizeof(buf)-1));
|
||||
bool enable;
|
||||
|
||||
if (test_bit(HCI_UP, &hdev->flags))
|
||||
return -EBUSY;
|
||||
|
||||
if (copy_from_user(buf, user_buf, buf_size))
|
||||
return -EFAULT;
|
||||
|
||||
buf[buf_size] = '\0';
|
||||
if (strtobool(buf, &enable))
|
||||
return -EINVAL;
|
||||
|
||||
if (enable == test_bit(HCI_FORCE_SC, &hdev->dbg_flags))
|
||||
return -EALREADY;
|
||||
|
||||
change_bit(HCI_FORCE_SC, &hdev->dbg_flags);
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
static const struct file_operations force_sc_support_fops = {
|
||||
.open = simple_open,
|
||||
.read = force_sc_support_read,
|
||||
.write = force_sc_support_write,
|
||||
.llseek = default_llseek,
|
||||
};
|
||||
|
||||
static ssize_t force_lesc_support_read(struct file *file,
|
||||
char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[3];
|
||||
|
||||
buf[0] = test_bit(HCI_FORCE_LESC, &hdev->dbg_flags) ? 'Y': 'N';
|
||||
buf[1] = '\n';
|
||||
buf[2] = '\0';
|
||||
return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
|
||||
}
|
||||
|
||||
static ssize_t force_lesc_support_write(struct file *file,
|
||||
const char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[32];
|
||||
size_t buf_size = min(count, (sizeof(buf)-1));
|
||||
bool enable;
|
||||
|
||||
if (copy_from_user(buf, user_buf, buf_size))
|
||||
return -EFAULT;
|
||||
|
||||
buf[buf_size] = '\0';
|
||||
if (strtobool(buf, &enable))
|
||||
return -EINVAL;
|
||||
|
||||
if (enable == test_bit(HCI_FORCE_LESC, &hdev->dbg_flags))
|
||||
return -EALREADY;
|
||||
|
||||
change_bit(HCI_FORCE_LESC, &hdev->dbg_flags);
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
static const struct file_operations force_lesc_support_fops = {
|
||||
.open = simple_open,
|
||||
.read = force_lesc_support_read,
|
||||
.write = force_lesc_support_write,
|
||||
.llseek = default_llseek,
|
||||
};
|
||||
|
||||
static int idle_timeout_set(void *data, u64 val)
|
||||
{
|
||||
struct hci_dev *hdev = data;
|
||||
|
@ -560,20 +474,9 @@ void hci_debugfs_create_bredr(struct hci_dev *hdev)
|
|||
debugfs_create_file("voice_setting", 0444, hdev->debugfs, hdev,
|
||||
&voice_setting_fops);
|
||||
|
||||
if (lmp_ssp_capable(hdev)) {
|
||||
if (lmp_ssp_capable(hdev))
|
||||
debugfs_create_file("auto_accept_delay", 0644, hdev->debugfs,
|
||||
hdev, &auto_accept_delay_fops);
|
||||
debugfs_create_file("sc_only_mode", 0444, hdev->debugfs,
|
||||
hdev, &sc_only_mode_fops);
|
||||
|
||||
debugfs_create_file("force_sc_support", 0644, hdev->debugfs,
|
||||
hdev, &force_sc_support_fops);
|
||||
|
||||
if (lmp_le_capable(hdev))
|
||||
debugfs_create_file("force_lesc_support", 0644,
|
||||
hdev->debugfs, hdev,
|
||||
&force_lesc_support_fops);
|
||||
}
|
||||
|
||||
if (lmp_sniff_capable(hdev)) {
|
||||
debugfs_create_file("idle_timeout", 0644, hdev->debugfs,
|
||||
|
|
|
@ -214,6 +214,40 @@ static void hci_cc_reset(struct hci_dev *hdev, struct sk_buff *skb)
|
|||
hci_bdaddr_list_clear(&hdev->le_white_list);
|
||||
}
|
||||
|
||||
static void hci_cc_read_stored_link_key(struct hci_dev *hdev,
|
||||
struct sk_buff *skb)
|
||||
{
|
||||
struct hci_rp_read_stored_link_key *rp = (void *)skb->data;
|
||||
struct hci_cp_read_stored_link_key *sent;
|
||||
|
||||
BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
|
||||
|
||||
sent = hci_sent_cmd_data(hdev, HCI_OP_READ_STORED_LINK_KEY);
|
||||
if (!sent)
|
||||
return;
|
||||
|
||||
if (!rp->status && sent->read_all == 0x01) {
|
||||
hdev->stored_max_keys = rp->max_keys;
|
||||
hdev->stored_num_keys = rp->num_keys;
|
||||
}
|
||||
}
|
||||
|
||||
static void hci_cc_delete_stored_link_key(struct hci_dev *hdev,
|
||||
struct sk_buff *skb)
|
||||
{
|
||||
struct hci_rp_delete_stored_link_key *rp = (void *)skb->data;
|
||||
|
||||
BT_DBG("%s status 0x%2.2x", hdev->name, rp->status);
|
||||
|
||||
if (rp->status)
|
||||
return;
|
||||
|
||||
if (rp->num_keys <= hdev->stored_num_keys)
|
||||
hdev->stored_num_keys -= rp->num_keys;
|
||||
else
|
||||
hdev->stored_num_keys = 0;
|
||||
}
|
||||
|
||||
static void hci_cc_write_local_name(struct hci_dev *hdev, struct sk_buff *skb)
|
||||
{
|
||||
__u8 status = *((__u8 *) skb->data);
|
||||
|
@ -2714,6 +2748,14 @@ static void hci_cmd_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
|
|||
hci_cc_reset(hdev, skb);
|
||||
break;
|
||||
|
||||
case HCI_OP_READ_STORED_LINK_KEY:
|
||||
hci_cc_read_stored_link_key(hdev, skb);
|
||||
break;
|
||||
|
||||
case HCI_OP_DELETE_STORED_LINK_KEY:
|
||||
hci_cc_delete_stored_link_key(hdev, skb);
|
||||
break;
|
||||
|
||||
case HCI_OP_WRITE_LOCAL_NAME:
|
||||
hci_cc_write_local_name(hdev, skb);
|
||||
break;
|
||||
|
|
|
@ -533,7 +533,8 @@ void __hci_update_background_scan(struct hci_request *req)
|
|||
}
|
||||
}
|
||||
|
||||
static void update_background_scan_complete(struct hci_dev *hdev, u8 status)
|
||||
static void update_background_scan_complete(struct hci_dev *hdev, u8 status,
|
||||
u16 opcode)
|
||||
{
|
||||
if (status)
|
||||
BT_DBG("HCI request failed to update background scanning: "
|
||||
|
|
|
@ -216,11 +216,39 @@ void hci_send_to_control(struct sk_buff *skb, struct sock *skip_sk)
|
|||
read_unlock(&hci_sk_list.lock);
|
||||
}
|
||||
|
||||
static void queue_monitor_skb(struct sk_buff *skb)
|
||||
{
|
||||
struct sock *sk;
|
||||
|
||||
BT_DBG("len %d", skb->len);
|
||||
|
||||
read_lock(&hci_sk_list.lock);
|
||||
|
||||
sk_for_each(sk, &hci_sk_list.head) {
|
||||
struct sk_buff *nskb;
|
||||
|
||||
if (sk->sk_state != BT_BOUND)
|
||||
continue;
|
||||
|
||||
if (hci_pi(sk)->channel != HCI_CHANNEL_MONITOR)
|
||||
continue;
|
||||
|
||||
nskb = skb_clone(skb, GFP_ATOMIC);
|
||||
if (!nskb)
|
||||
continue;
|
||||
|
||||
if (sock_queue_rcv_skb(sk, nskb))
|
||||
kfree_skb(nskb);
|
||||
}
|
||||
|
||||
read_unlock(&hci_sk_list.lock);
|
||||
}
|
||||
|
||||
/* Send frame to monitor socket */
|
||||
void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb)
|
||||
{
|
||||
struct sock *sk;
|
||||
struct sk_buff *skb_copy = NULL;
|
||||
struct hci_mon_hdr *hdr;
|
||||
__le16 opcode;
|
||||
|
||||
if (!atomic_read(&monitor_promisc))
|
||||
|
@ -251,74 +279,21 @@ void hci_send_to_monitor(struct hci_dev *hdev, struct sk_buff *skb)
|
|||
return;
|
||||
}
|
||||
|
||||
read_lock(&hci_sk_list.lock);
|
||||
/* Create a private copy with headroom */
|
||||
skb_copy = __pskb_copy_fclone(skb, HCI_MON_HDR_SIZE, GFP_ATOMIC, true);
|
||||
if (!skb_copy)
|
||||
return;
|
||||
|
||||
sk_for_each(sk, &hci_sk_list.head) {
|
||||
struct sk_buff *nskb;
|
||||
|
||||
if (sk->sk_state != BT_BOUND)
|
||||
continue;
|
||||
|
||||
if (hci_pi(sk)->channel != HCI_CHANNEL_MONITOR)
|
||||
continue;
|
||||
|
||||
if (!skb_copy) {
|
||||
struct hci_mon_hdr *hdr;
|
||||
|
||||
/* Create a private copy with headroom */
|
||||
skb_copy = __pskb_copy_fclone(skb, HCI_MON_HDR_SIZE,
|
||||
GFP_ATOMIC, true);
|
||||
if (!skb_copy)
|
||||
continue;
|
||||
|
||||
/* Put header before the data */
|
||||
hdr = (void *) skb_push(skb_copy, HCI_MON_HDR_SIZE);
|
||||
hdr->opcode = opcode;
|
||||
hdr->index = cpu_to_le16(hdev->id);
|
||||
hdr->len = cpu_to_le16(skb->len);
|
||||
}
|
||||
|
||||
nskb = skb_clone(skb_copy, GFP_ATOMIC);
|
||||
if (!nskb)
|
||||
continue;
|
||||
|
||||
if (sock_queue_rcv_skb(sk, nskb))
|
||||
kfree_skb(nskb);
|
||||
}
|
||||
|
||||
read_unlock(&hci_sk_list.lock);
|
||||
/* Put header before the data */
|
||||
hdr = (void *) skb_push(skb_copy, HCI_MON_HDR_SIZE);
|
||||
hdr->opcode = opcode;
|
||||
hdr->index = cpu_to_le16(hdev->id);
|
||||
hdr->len = cpu_to_le16(skb->len);
|
||||
|
||||
queue_monitor_skb(skb_copy);
|
||||
kfree_skb(skb_copy);
|
||||
}
|
||||
|
||||
static void send_monitor_event(struct sk_buff *skb)
|
||||
{
|
||||
struct sock *sk;
|
||||
|
||||
BT_DBG("len %d", skb->len);
|
||||
|
||||
read_lock(&hci_sk_list.lock);
|
||||
|
||||
sk_for_each(sk, &hci_sk_list.head) {
|
||||
struct sk_buff *nskb;
|
||||
|
||||
if (sk->sk_state != BT_BOUND)
|
||||
continue;
|
||||
|
||||
if (hci_pi(sk)->channel != HCI_CHANNEL_MONITOR)
|
||||
continue;
|
||||
|
||||
nskb = skb_clone(skb, GFP_ATOMIC);
|
||||
if (!nskb)
|
||||
continue;
|
||||
|
||||
if (sock_queue_rcv_skb(sk, nskb))
|
||||
kfree_skb(nskb);
|
||||
}
|
||||
|
||||
read_unlock(&hci_sk_list.lock);
|
||||
}
|
||||
|
||||
static struct sk_buff *create_monitor_event(struct hci_dev *hdev, int event)
|
||||
{
|
||||
struct hci_mon_hdr *hdr;
|
||||
|
@ -422,7 +397,7 @@ void hci_sock_dev_event(struct hci_dev *hdev, int event)
|
|||
|
||||
skb = create_monitor_event(hdev, event);
|
||||
if (skb) {
|
||||
send_monitor_event(skb);
|
||||
queue_monitor_skb(skb);
|
||||
kfree_skb(skb);
|
||||
}
|
||||
}
|
||||
|
@ -1230,6 +1205,8 @@ int __init hci_sock_init(void)
|
|||
{
|
||||
int err;
|
||||
|
||||
BUILD_BUG_ON(sizeof(struct sockaddr_hci) > sizeof(struct sockaddr));
|
||||
|
||||
err = proto_register(&hci_sk_proto, 0);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
|
|
@ -63,10 +63,10 @@ static void l2cap_send_disconn_req(struct l2cap_chan *chan, int err);
|
|||
static void l2cap_tx(struct l2cap_chan *chan, struct l2cap_ctrl *control,
|
||||
struct sk_buff_head *skbs, u8 event);
|
||||
|
||||
static inline __u8 bdaddr_type(struct hci_conn *hcon, __u8 type)
|
||||
static inline u8 bdaddr_type(u8 link_type, u8 bdaddr_type)
|
||||
{
|
||||
if (hcon->type == LE_LINK) {
|
||||
if (type == ADDR_LE_DEV_PUBLIC)
|
||||
if (link_type == LE_LINK) {
|
||||
if (bdaddr_type == ADDR_LE_DEV_PUBLIC)
|
||||
return BDADDR_LE_PUBLIC;
|
||||
else
|
||||
return BDADDR_LE_RANDOM;
|
||||
|
@ -75,6 +75,16 @@ static inline __u8 bdaddr_type(struct hci_conn *hcon, __u8 type)
|
|||
return BDADDR_BREDR;
|
||||
}
|
||||
|
||||
static inline u8 bdaddr_src_type(struct hci_conn *hcon)
|
||||
{
|
||||
return bdaddr_type(hcon->type, hcon->src_type);
|
||||
}
|
||||
|
||||
static inline u8 bdaddr_dst_type(struct hci_conn *hcon)
|
||||
{
|
||||
return bdaddr_type(hcon->type, hcon->dst_type);
|
||||
}
|
||||
|
||||
/* ---- L2CAP channels ---- */
|
||||
|
||||
static struct l2cap_chan *__l2cap_get_chan_by_dcid(struct l2cap_conn *conn,
|
||||
|
@ -646,7 +656,7 @@ static void l2cap_conn_update_id_addr(struct work_struct *work)
|
|||
list_for_each_entry(chan, &conn->chan_l, list) {
|
||||
l2cap_chan_lock(chan);
|
||||
bacpy(&chan->dst, &hcon->dst);
|
||||
chan->dst_type = bdaddr_type(hcon, hcon->dst_type);
|
||||
chan->dst_type = bdaddr_dst_type(hcon);
|
||||
l2cap_chan_unlock(chan);
|
||||
}
|
||||
|
||||
|
@ -3790,8 +3800,8 @@ static struct l2cap_chan *l2cap_connect(struct l2cap_conn *conn,
|
|||
|
||||
bacpy(&chan->src, &conn->hcon->src);
|
||||
bacpy(&chan->dst, &conn->hcon->dst);
|
||||
chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
|
||||
chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
|
||||
chan->src_type = bdaddr_src_type(conn->hcon);
|
||||
chan->dst_type = bdaddr_dst_type(conn->hcon);
|
||||
chan->psm = psm;
|
||||
chan->dcid = scid;
|
||||
chan->local_amp_id = amp_id;
|
||||
|
@ -5441,8 +5451,8 @@ static int l2cap_le_connect_req(struct l2cap_conn *conn,
|
|||
|
||||
bacpy(&chan->src, &conn->hcon->src);
|
||||
bacpy(&chan->dst, &conn->hcon->dst);
|
||||
chan->src_type = bdaddr_type(conn->hcon, conn->hcon->src_type);
|
||||
chan->dst_type = bdaddr_type(conn->hcon, conn->hcon->dst_type);
|
||||
chan->src_type = bdaddr_src_type(conn->hcon);
|
||||
chan->dst_type = bdaddr_dst_type(conn->hcon);
|
||||
chan->psm = psm;
|
||||
chan->dcid = scid;
|
||||
chan->omtu = mtu;
|
||||
|
@ -6881,7 +6891,7 @@ static void l2cap_recv_frame(struct l2cap_conn *conn, struct sk_buff *skb)
|
|||
*/
|
||||
if (hcon->type == LE_LINK &&
|
||||
hci_bdaddr_list_lookup(&hcon->hdev->blacklist, &hcon->dst,
|
||||
bdaddr_type(hcon, hcon->dst_type))) {
|
||||
bdaddr_dst_type(hcon))) {
|
||||
kfree_skb(skb);
|
||||
return;
|
||||
}
|
||||
|
@ -6968,7 +6978,7 @@ static struct l2cap_conn *l2cap_conn_add(struct hci_conn *hcon)
|
|||
|
||||
if (test_bit(HCI_LE_ENABLED, &hcon->hdev->dev_flags) &&
|
||||
(bredr_sc_enabled(hcon->hdev) ||
|
||||
test_bit(HCI_FORCE_LESC, &hcon->hdev->dbg_flags)))
|
||||
test_bit(HCI_FORCE_BREDR_SMP, &hcon->hdev->dbg_flags)))
|
||||
conn->local_fixed_chan |= L2CAP_FC_SMP_BREDR;
|
||||
|
||||
mutex_init(&conn->ident_lock);
|
||||
|
@ -7123,7 +7133,7 @@ int l2cap_chan_connect(struct l2cap_chan *chan, __le16 psm, u16 cid,
|
|||
|
||||
/* Update source addr of the socket */
|
||||
bacpy(&chan->src, &hcon->src);
|
||||
chan->src_type = bdaddr_type(hcon, hcon->src_type);
|
||||
chan->src_type = bdaddr_src_type(hcon);
|
||||
|
||||
__l2cap_chan_add(conn, chan);
|
||||
|
||||
|
@ -7197,8 +7207,10 @@ int l2cap_connect_ind(struct hci_dev *hdev, bdaddr_t *bdaddr)
|
|||
* global list (by passing NULL as first parameter).
|
||||
*/
|
||||
static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
|
||||
bdaddr_t *src, u8 link_type)
|
||||
struct hci_conn *hcon)
|
||||
{
|
||||
u8 src_type = bdaddr_src_type(hcon);
|
||||
|
||||
read_lock(&chan_list_lock);
|
||||
|
||||
if (c)
|
||||
|
@ -7211,11 +7223,9 @@ static struct l2cap_chan *l2cap_global_fixed_chan(struct l2cap_chan *c,
|
|||
continue;
|
||||
if (c->state != BT_LISTEN)
|
||||
continue;
|
||||
if (bacmp(&c->src, src) && bacmp(&c->src, BDADDR_ANY))
|
||||
if (bacmp(&c->src, &hcon->src) && bacmp(&c->src, BDADDR_ANY))
|
||||
continue;
|
||||
if (link_type == ACL_LINK && c->src_type != BDADDR_BREDR)
|
||||
continue;
|
||||
if (link_type == LE_LINK && c->src_type == BDADDR_BREDR)
|
||||
if (src_type != c->src_type)
|
||||
continue;
|
||||
|
||||
l2cap_chan_hold(c);
|
||||
|
@ -7246,7 +7256,7 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
|
|||
if (!conn)
|
||||
return;
|
||||
|
||||
dst_type = bdaddr_type(hcon, hcon->dst_type);
|
||||
dst_type = bdaddr_dst_type(hcon);
|
||||
|
||||
/* If device is blocked, do not create channels for it */
|
||||
if (hci_bdaddr_list_lookup(&hdev->blacklist, &hcon->dst, dst_type))
|
||||
|
@ -7257,7 +7267,7 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
|
|||
* we left off, because the list lock would prevent calling the
|
||||
* potentially sleeping l2cap_chan_lock() function.
|
||||
*/
|
||||
pchan = l2cap_global_fixed_chan(NULL, &hdev->bdaddr, hcon->type);
|
||||
pchan = l2cap_global_fixed_chan(NULL, hcon);
|
||||
while (pchan) {
|
||||
struct l2cap_chan *chan, *next;
|
||||
|
||||
|
@ -7270,7 +7280,7 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
|
|||
if (chan) {
|
||||
bacpy(&chan->src, &hcon->src);
|
||||
bacpy(&chan->dst, &hcon->dst);
|
||||
chan->src_type = bdaddr_type(hcon, hcon->src_type);
|
||||
chan->src_type = bdaddr_src_type(hcon);
|
||||
chan->dst_type = dst_type;
|
||||
|
||||
__l2cap_chan_add(conn, chan);
|
||||
|
@ -7278,8 +7288,7 @@ void l2cap_connect_cfm(struct hci_conn *hcon, u8 status)
|
|||
|
||||
l2cap_chan_unlock(pchan);
|
||||
next:
|
||||
next = l2cap_global_fixed_chan(pchan, &hdev->bdaddr,
|
||||
hcon->type);
|
||||
next = l2cap_global_fixed_chan(pchan, hcon);
|
||||
l2cap_chan_put(pchan);
|
||||
pchan = next;
|
||||
}
|
||||
|
@ -7527,8 +7536,8 @@ static int l2cap_debugfs_show(struct seq_file *f, void *p)
|
|||
read_lock(&chan_list_lock);
|
||||
|
||||
list_for_each_entry(c, &chan_list, global_l) {
|
||||
seq_printf(f, "%pMR %pMR %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
|
||||
&c->src, &c->dst,
|
||||
seq_printf(f, "%pMR (%u) %pMR (%u) %d %d 0x%4.4x 0x%4.4x %d %d %d %d\n",
|
||||
&c->src, c->src_type, &c->dst, c->dst_type,
|
||||
c->state, __le16_to_cpu(c->psm),
|
||||
c->scid, c->dcid, c->imtu, c->omtu,
|
||||
c->sec_level, c->mode);
|
||||
|
|
|
@ -1614,6 +1614,8 @@ int __init l2cap_init_sockets(void)
|
|||
{
|
||||
int err;
|
||||
|
||||
BUILD_BUG_ON(sizeof(struct sockaddr_l2) > sizeof(struct sockaddr));
|
||||
|
||||
err = proto_register(&l2cap_proto, 0);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
|
|
@ -570,8 +570,7 @@ static u32 get_supported_settings(struct hci_dev *hdev)
|
|||
settings |= MGMT_SETTING_HS;
|
||||
}
|
||||
|
||||
if (lmp_sc_capable(hdev) ||
|
||||
test_bit(HCI_FORCE_SC, &hdev->dbg_flags))
|
||||
if (lmp_sc_capable(hdev))
|
||||
settings |= MGMT_SETTING_SECURE_CONN;
|
||||
}
|
||||
|
||||
|
@ -1252,7 +1251,7 @@ static int send_settings_rsp(struct sock *sk, u16 opcode, struct hci_dev *hdev)
|
|||
sizeof(settings));
|
||||
}
|
||||
|
||||
static void clean_up_hci_complete(struct hci_dev *hdev, u8 status)
|
||||
static void clean_up_hci_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
BT_DBG("%s status 0x%02x", hdev->name, status);
|
||||
|
||||
|
@ -1519,7 +1518,8 @@ static u8 mgmt_le_support(struct hci_dev *hdev)
|
|||
return MGMT_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static void set_discoverable_complete(struct hci_dev *hdev, u8 status)
|
||||
static void set_discoverable_complete(struct hci_dev *hdev, u8 status,
|
||||
u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
struct mgmt_mode *cp;
|
||||
|
@ -1778,7 +1778,8 @@ static void write_fast_connectable(struct hci_request *req, bool enable)
|
|||
hci_req_add(req, HCI_OP_WRITE_PAGE_SCAN_TYPE, 1, &type);
|
||||
}
|
||||
|
||||
static void set_connectable_complete(struct hci_dev *hdev, u8 status)
|
||||
static void set_connectable_complete(struct hci_dev *hdev, u8 status,
|
||||
u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
struct mgmt_mode *cp;
|
||||
|
@ -2196,7 +2197,7 @@ static int set_hs(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
|
|||
return err;
|
||||
}
|
||||
|
||||
static void le_enable_complete(struct hci_dev *hdev, u8 status)
|
||||
static void le_enable_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct cmd_lookup match = { NULL, hdev };
|
||||
|
||||
|
@ -2386,7 +2387,7 @@ static void mgmt_class_complete(struct hci_dev *hdev, u16 mgmt_op, u8 status)
|
|||
hci_dev_unlock(hdev);
|
||||
}
|
||||
|
||||
static void add_uuid_complete(struct hci_dev *hdev, u8 status)
|
||||
static void add_uuid_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
BT_DBG("status 0x%02x", status);
|
||||
|
||||
|
@ -2465,7 +2466,7 @@ static bool enable_service_cache(struct hci_dev *hdev)
|
|||
return false;
|
||||
}
|
||||
|
||||
static void remove_uuid_complete(struct hci_dev *hdev, u8 status)
|
||||
static void remove_uuid_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
BT_DBG("status 0x%02x", status);
|
||||
|
||||
|
@ -2550,7 +2551,7 @@ static int remove_uuid(struct sock *sk, struct hci_dev *hdev, void *data,
|
|||
return err;
|
||||
}
|
||||
|
||||
static void set_class_complete(struct hci_dev *hdev, u8 status)
|
||||
static void set_class_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
BT_DBG("status 0x%02x", status);
|
||||
|
||||
|
@ -3484,7 +3485,7 @@ static void update_name(struct hci_request *req)
|
|||
hci_req_add(req, HCI_OP_WRITE_LOCAL_NAME, sizeof(cp), &cp);
|
||||
}
|
||||
|
||||
static void set_name_complete(struct hci_dev *hdev, u8 status)
|
||||
static void set_name_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct mgmt_cp_set_local_name *cp;
|
||||
struct pending_cmd *cmd;
|
||||
|
@ -3835,7 +3836,8 @@ static bool trigger_discovery(struct hci_request *req, u8 *status)
|
|||
return true;
|
||||
}
|
||||
|
||||
static void start_discovery_complete(struct hci_dev *hdev, u8 status)
|
||||
static void start_discovery_complete(struct hci_dev *hdev, u8 status,
|
||||
u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
unsigned long timeout;
|
||||
|
@ -4064,7 +4066,7 @@ static int start_service_discovery(struct sock *sk, struct hci_dev *hdev,
|
|||
return err;
|
||||
}
|
||||
|
||||
static void stop_discovery_complete(struct hci_dev *hdev, u8 status)
|
||||
static void stop_discovery_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
|
||||
|
@ -4290,7 +4292,8 @@ static int set_device_id(struct sock *sk, struct hci_dev *hdev, void *data,
|
|||
return err;
|
||||
}
|
||||
|
||||
static void set_advertising_complete(struct hci_dev *hdev, u8 status)
|
||||
static void set_advertising_complete(struct hci_dev *hdev, u8 status,
|
||||
u16 opcode)
|
||||
{
|
||||
struct cmd_lookup match = { NULL, hdev };
|
||||
|
||||
|
@ -4497,7 +4500,8 @@ static int set_scan_params(struct sock *sk, struct hci_dev *hdev,
|
|||
return err;
|
||||
}
|
||||
|
||||
static void fast_connectable_complete(struct hci_dev *hdev, u8 status)
|
||||
static void fast_connectable_complete(struct hci_dev *hdev, u8 status,
|
||||
u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
|
||||
|
@ -4595,7 +4599,7 @@ static int set_fast_connectable(struct sock *sk, struct hci_dev *hdev,
|
|||
return err;
|
||||
}
|
||||
|
||||
static void set_bredr_complete(struct hci_dev *hdev, u8 status)
|
||||
static void set_bredr_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
|
||||
|
@ -4679,6 +4683,21 @@ static int set_bredr(struct sock *sk, struct hci_dev *hdev, void *data, u16 len)
|
|||
err = cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR,
|
||||
MGMT_STATUS_REJECTED);
|
||||
goto unlock;
|
||||
} else {
|
||||
/* When configuring a dual-mode controller to operate
|
||||
* with LE only and using a static address, then switching
|
||||
* BR/EDR back on is not allowed.
|
||||
*
|
||||
* Dual-mode controllers shall operate with the public
|
||||
* address as its identity address for BR/EDR and LE. So
|
||||
* reject the attempt to create an invalid configuration.
|
||||
*/
|
||||
if (!test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags) &&
|
||||
bacmp(&hdev->static_addr, BDADDR_ANY)) {
|
||||
err = cmd_status(sk, hdev->id, MGMT_OP_SET_BREDR,
|
||||
MGMT_STATUS_REJECTED);
|
||||
goto unlock;
|
||||
}
|
||||
}
|
||||
|
||||
if (mgmt_pending_find(MGMT_OP_SET_BREDR, hdev)) {
|
||||
|
@ -4727,8 +4746,8 @@ static int set_secure_conn(struct sock *sk, struct hci_dev *hdev,
|
|||
|
||||
BT_DBG("request for %s", hdev->name);
|
||||
|
||||
if (!test_bit(HCI_LE_ENABLED, &hdev->dev_flags) &&
|
||||
!lmp_sc_capable(hdev) && !test_bit(HCI_FORCE_SC, &hdev->dbg_flags))
|
||||
if (!lmp_sc_capable(hdev) &&
|
||||
!test_bit(HCI_LE_ENABLED, &hdev->dev_flags))
|
||||
return cmd_status(sk, hdev->id, MGMT_OP_SET_SECURE_CONN,
|
||||
MGMT_STATUS_NOT_SUPPORTED);
|
||||
|
||||
|
@ -4738,9 +4757,7 @@ static int set_secure_conn(struct sock *sk, struct hci_dev *hdev,
|
|||
|
||||
hci_dev_lock(hdev);
|
||||
|
||||
if (!hdev_is_powered(hdev) ||
|
||||
(!lmp_sc_capable(hdev) &&
|
||||
!test_bit(HCI_FORCE_SC, &hdev->dbg_flags)) ||
|
||||
if (!hdev_is_powered(hdev) || !lmp_sc_capable(hdev) ||
|
||||
!test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags)) {
|
||||
bool changed;
|
||||
|
||||
|
@ -5122,7 +5139,8 @@ static int conn_info_cmd_complete(struct pending_cmd *cmd, u8 status)
|
|||
return err;
|
||||
}
|
||||
|
||||
static void conn_info_refresh_complete(struct hci_dev *hdev, u8 hci_status)
|
||||
static void conn_info_refresh_complete(struct hci_dev *hdev, u8 hci_status,
|
||||
u16 opcode)
|
||||
{
|
||||
struct hci_cp_read_rssi *cp;
|
||||
struct pending_cmd *cmd;
|
||||
|
@ -5329,7 +5347,7 @@ static int clock_info_cmd_complete(struct pending_cmd *cmd, u8 status)
|
|||
return err;
|
||||
}
|
||||
|
||||
static void get_clock_info_complete(struct hci_dev *hdev, u8 status)
|
||||
static void get_clock_info_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct hci_cp_read_clock *hci_cp;
|
||||
struct pending_cmd *cmd;
|
||||
|
@ -5507,7 +5525,7 @@ static void device_added(struct sock *sk, struct hci_dev *hdev,
|
|||
mgmt_event(MGMT_EV_DEVICE_ADDED, hdev, &ev, sizeof(ev), sk);
|
||||
}
|
||||
|
||||
static void add_device_complete(struct hci_dev *hdev, u8 status)
|
||||
static void add_device_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
|
||||
|
@ -5630,7 +5648,7 @@ static void device_removed(struct sock *sk, struct hci_dev *hdev,
|
|||
mgmt_event(MGMT_EV_DEVICE_REMOVED, hdev, &ev, sizeof(ev), sk);
|
||||
}
|
||||
|
||||
static void remove_device_complete(struct hci_dev *hdev, u8 status)
|
||||
static void remove_device_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct pending_cmd *cmd;
|
||||
|
||||
|
@ -6208,12 +6226,21 @@ static void restart_le_actions(struct hci_request *req)
|
|||
__hci_update_background_scan(req);
|
||||
}
|
||||
|
||||
static void powered_complete(struct hci_dev *hdev, u8 status)
|
||||
static void powered_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
struct cmd_lookup match = { NULL, hdev };
|
||||
|
||||
BT_DBG("status 0x%02x", status);
|
||||
|
||||
if (!status) {
|
||||
/* Register the available SMP channels (BR/EDR and LE) only
|
||||
* when successfully powering on the controller. This late
|
||||
* registration is required so that LE SMP can clearly
|
||||
* decide if the public address or static address is used.
|
||||
*/
|
||||
smp_register(hdev);
|
||||
}
|
||||
|
||||
hci_dev_lock(hdev);
|
||||
|
||||
mgmt_pending_foreach(MGMT_OP_SET_POWERED, hdev, settings_rsp, &match);
|
||||
|
@ -7319,7 +7346,7 @@ void mgmt_discovering(struct hci_dev *hdev, u8 discovering)
|
|||
mgmt_event(MGMT_EV_DISCOVERING, hdev, &ev, sizeof(ev), NULL);
|
||||
}
|
||||
|
||||
static void adv_enable_complete(struct hci_dev *hdev, u8 status)
|
||||
static void adv_enable_complete(struct hci_dev *hdev, u8 status, u16 opcode)
|
||||
{
|
||||
BT_DBG("%s status %u", hdev->name, status);
|
||||
}
|
||||
|
|
|
@ -1058,6 +1058,8 @@ int __init rfcomm_init_sockets(void)
|
|||
{
|
||||
int err;
|
||||
|
||||
BUILD_BUG_ON(sizeof(struct sockaddr_rc) > sizeof(struct sockaddr));
|
||||
|
||||
err = proto_register(&rfcomm_proto, 0);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
|
|
@ -1184,6 +1184,8 @@ int __init sco_init(void)
|
|||
{
|
||||
int err;
|
||||
|
||||
BUILD_BUG_ON(sizeof(struct sockaddr_sco) > sizeof(struct sockaddr));
|
||||
|
||||
err = proto_register(&sco_proto, 0);
|
||||
if (err < 0)
|
||||
return err;
|
||||
|
|
|
@ -184,7 +184,7 @@ static int __init test_ecdh(void)
|
|||
delta = ktime_sub(rettime, calltime);
|
||||
duration = (unsigned long long) ktime_to_ns(delta) >> 10;
|
||||
|
||||
BT_INFO("ECDH test passed in %lld usecs", duration);
|
||||
BT_INFO("ECDH test passed in %llu usecs", duration);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
|
@ -20,6 +20,7 @@
|
|||
SOFTWARE IS DISCLAIMED.
|
||||
*/
|
||||
|
||||
#include <linux/debugfs.h>
|
||||
#include <linux/crypto.h>
|
||||
#include <linux/scatterlist.h>
|
||||
#include <crypto/b128ops.h>
|
||||
|
@ -299,7 +300,7 @@ static int smp_f6(struct crypto_hash *tfm_cmac, const u8 w[16],
|
|||
if (err)
|
||||
return err;
|
||||
|
||||
BT_DBG("res %16phN", res);
|
||||
SMP_DBG("res %16phN", res);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
@ -1675,7 +1676,7 @@ static u8 smp_cmd_pairing_req(struct l2cap_conn *conn, struct sk_buff *skb)
|
|||
if (conn->hcon->type == ACL_LINK) {
|
||||
/* We must have a BR/EDR SC link */
|
||||
if (!test_bit(HCI_CONN_AES_CCM, &conn->hcon->flags) &&
|
||||
!test_bit(HCI_FORCE_LESC, &hdev->dbg_flags))
|
||||
!test_bit(HCI_FORCE_BREDR_SMP, &hdev->dbg_flags))
|
||||
return SMP_CROSS_TRANSP_NOT_ALLOWED;
|
||||
|
||||
set_bit(SMP_FLAG_SC, &smp->flags);
|
||||
|
@ -2304,8 +2305,12 @@ static int smp_cmd_ident_addr_info(struct l2cap_conn *conn,
|
|||
* implementations are not known of and in order to not over
|
||||
* complicate our implementation, simply pretend that we never
|
||||
* received an IRK for such a device.
|
||||
*
|
||||
* The Identity Address must also be a Static Random or Public
|
||||
* Address, which hci_is_identity_address() checks for.
|
||||
*/
|
||||
if (!bacmp(&info->bdaddr, BDADDR_ANY)) {
|
||||
if (!bacmp(&info->bdaddr, BDADDR_ANY) ||
|
||||
!hci_is_identity_address(&info->bdaddr, info->addr_type)) {
|
||||
BT_ERR("Ignoring IRK with no identity address");
|
||||
goto distribute;
|
||||
}
|
||||
|
@ -2738,7 +2743,7 @@ static void bredr_pairing(struct l2cap_chan *chan)
|
|||
|
||||
/* BR/EDR must use Secure Connections for SMP */
|
||||
if (!test_bit(HCI_CONN_AES_CCM, &hcon->flags) &&
|
||||
!test_bit(HCI_FORCE_LESC, &hdev->dbg_flags))
|
||||
!test_bit(HCI_FORCE_BREDR_SMP, &hdev->dbg_flags))
|
||||
return;
|
||||
|
||||
/* If our LE support is not enabled don't do anything */
|
||||
|
@ -2945,11 +2950,30 @@ static struct l2cap_chan *smp_add_cid(struct hci_dev *hdev, u16 cid)
|
|||
|
||||
l2cap_chan_set_defaults(chan);
|
||||
|
||||
bacpy(&chan->src, &hdev->bdaddr);
|
||||
if (cid == L2CAP_CID_SMP)
|
||||
chan->src_type = BDADDR_LE_PUBLIC;
|
||||
else
|
||||
if (cid == L2CAP_CID_SMP) {
|
||||
/* If usage of static address is forced or if the devices
|
||||
* does not have a public address, then listen on the static
|
||||
* address.
|
||||
*
|
||||
* In case BR/EDR has been disabled on a dual-mode controller
|
||||
* and a static address has been configued, then listen on
|
||||
* the static address instead.
|
||||
*/
|
||||
if (test_bit(HCI_FORCE_STATIC_ADDR, &hdev->dbg_flags) ||
|
||||
!bacmp(&hdev->bdaddr, BDADDR_ANY) ||
|
||||
(!test_bit(HCI_BREDR_ENABLED, &hdev->dev_flags) &&
|
||||
bacmp(&hdev->static_addr, BDADDR_ANY))) {
|
||||
bacpy(&chan->src, &hdev->static_addr);
|
||||
chan->src_type = BDADDR_LE_RANDOM;
|
||||
} else {
|
||||
bacpy(&chan->src, &hdev->bdaddr);
|
||||
chan->src_type = BDADDR_LE_PUBLIC;
|
||||
}
|
||||
} else {
|
||||
bacpy(&chan->src, &hdev->bdaddr);
|
||||
chan->src_type = BDADDR_BREDR;
|
||||
}
|
||||
|
||||
chan->state = BT_LISTEN;
|
||||
chan->mode = L2CAP_MODE_BASIC;
|
||||
chan->imtu = L2CAP_DEFAULT_MTU;
|
||||
|
@ -2976,21 +3000,108 @@ static void smp_del_chan(struct l2cap_chan *chan)
|
|||
l2cap_chan_put(chan);
|
||||
}
|
||||
|
||||
static ssize_t force_bredr_smp_read(struct file *file,
|
||||
char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[3];
|
||||
|
||||
buf[0] = test_bit(HCI_FORCE_BREDR_SMP, &hdev->dbg_flags) ? 'Y': 'N';
|
||||
buf[1] = '\n';
|
||||
buf[2] = '\0';
|
||||
return simple_read_from_buffer(user_buf, count, ppos, buf, 2);
|
||||
}
|
||||
|
||||
static ssize_t force_bredr_smp_write(struct file *file,
|
||||
const char __user *user_buf,
|
||||
size_t count, loff_t *ppos)
|
||||
{
|
||||
struct hci_dev *hdev = file->private_data;
|
||||
char buf[32];
|
||||
size_t buf_size = min(count, (sizeof(buf)-1));
|
||||
bool enable;
|
||||
|
||||
if (copy_from_user(buf, user_buf, buf_size))
|
||||
return -EFAULT;
|
||||
|
||||
buf[buf_size] = '\0';
|
||||
if (strtobool(buf, &enable))
|
||||
return -EINVAL;
|
||||
|
||||
if (enable == test_bit(HCI_FORCE_BREDR_SMP, &hdev->dbg_flags))
|
||||
return -EALREADY;
|
||||
|
||||
if (enable) {
|
||||
struct l2cap_chan *chan;
|
||||
|
||||
chan = smp_add_cid(hdev, L2CAP_CID_SMP_BREDR);
|
||||
if (IS_ERR(chan))
|
||||
return PTR_ERR(chan);
|
||||
|
||||
hdev->smp_bredr_data = chan;
|
||||
} else {
|
||||
struct l2cap_chan *chan;
|
||||
|
||||
chan = hdev->smp_bredr_data;
|
||||
hdev->smp_bredr_data = NULL;
|
||||
smp_del_chan(chan);
|
||||
}
|
||||
|
||||
change_bit(HCI_FORCE_BREDR_SMP, &hdev->dbg_flags);
|
||||
|
||||
return count;
|
||||
}
|
||||
|
||||
static const struct file_operations force_bredr_smp_fops = {
|
||||
.open = simple_open,
|
||||
.read = force_bredr_smp_read,
|
||||
.write = force_bredr_smp_write,
|
||||
.llseek = default_llseek,
|
||||
};
|
||||
|
||||
int smp_register(struct hci_dev *hdev)
|
||||
{
|
||||
struct l2cap_chan *chan;
|
||||
|
||||
BT_DBG("%s", hdev->name);
|
||||
|
||||
/* If the controller does not support Low Energy operation, then
|
||||
* there is also no need to register any SMP channel.
|
||||
*/
|
||||
if (!lmp_le_capable(hdev))
|
||||
return 0;
|
||||
|
||||
if (WARN_ON(hdev->smp_data)) {
|
||||
chan = hdev->smp_data;
|
||||
hdev->smp_data = NULL;
|
||||
smp_del_chan(chan);
|
||||
}
|
||||
|
||||
chan = smp_add_cid(hdev, L2CAP_CID_SMP);
|
||||
if (IS_ERR(chan))
|
||||
return PTR_ERR(chan);
|
||||
|
||||
hdev->smp_data = chan;
|
||||
|
||||
if (!lmp_sc_capable(hdev) &&
|
||||
!test_bit(HCI_FORCE_LESC, &hdev->dbg_flags))
|
||||
/* If the controller does not support BR/EDR Secure Connections
|
||||
* feature, then the BR/EDR SMP channel shall not be present.
|
||||
*
|
||||
* To test this with Bluetooth 4.0 controllers, create a debugfs
|
||||
* switch that allows forcing BR/EDR SMP support and accepting
|
||||
* cross-transport pairing on non-AES encrypted connections.
|
||||
*/
|
||||
if (!lmp_sc_capable(hdev)) {
|
||||
debugfs_create_file("force_bredr_smp", 0644, hdev->debugfs,
|
||||
hdev, &force_bredr_smp_fops);
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (WARN_ON(hdev->smp_bredr_data)) {
|
||||
chan = hdev->smp_bredr_data;
|
||||
hdev->smp_bredr_data = NULL;
|
||||
smp_del_chan(chan);
|
||||
}
|
||||
|
||||
chan = smp_add_cid(hdev, L2CAP_CID_SMP_BREDR);
|
||||
if (IS_ERR(chan)) {
|
||||
|
@ -3317,7 +3428,7 @@ static int __init run_selftests(struct crypto_blkcipher *tfm_aes,
|
|||
delta = ktime_sub(rettime, calltime);
|
||||
duration = (unsigned long long) ktime_to_ns(delta) >> 10;
|
||||
|
||||
BT_INFO("SMP test passed in %lld usecs", duration);
|
||||
BT_INFO("SMP test passed in %llu usecs", duration);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
72
net/ieee802154/6lowpan/6lowpan_i.h
Normal file
72
net/ieee802154/6lowpan/6lowpan_i.h
Normal file
|
@ -0,0 +1,72 @@
|
|||
#ifndef __IEEE802154_6LOWPAN_I_H__
|
||||
#define __IEEE802154_6LOWPAN_I_H__
|
||||
|
||||
#include <linux/list.h>
|
||||
|
||||
#include <net/ieee802154_netdev.h>
|
||||
#include <net/inet_frag.h>
|
||||
|
||||
struct lowpan_create_arg {
|
||||
u16 tag;
|
||||
u16 d_size;
|
||||
const struct ieee802154_addr *src;
|
||||
const struct ieee802154_addr *dst;
|
||||
};
|
||||
|
||||
/* Equivalent of ipv4 struct ip
|
||||
*/
|
||||
struct lowpan_frag_queue {
|
||||
struct inet_frag_queue q;
|
||||
|
||||
u16 tag;
|
||||
u16 d_size;
|
||||
struct ieee802154_addr saddr;
|
||||
struct ieee802154_addr daddr;
|
||||
};
|
||||
|
||||
static inline u32 ieee802154_addr_hash(const struct ieee802154_addr *a)
|
||||
{
|
||||
switch (a->mode) {
|
||||
case IEEE802154_ADDR_LONG:
|
||||
return (((__force u64)a->extended_addr) >> 32) ^
|
||||
(((__force u64)a->extended_addr) & 0xffffffff);
|
||||
case IEEE802154_ADDR_SHORT:
|
||||
return (__force u32)(a->short_addr);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
struct lowpan_dev_record {
|
||||
struct net_device *ldev;
|
||||
struct list_head list;
|
||||
};
|
||||
|
||||
/* private device info */
|
||||
struct lowpan_dev_info {
|
||||
struct net_device *real_dev; /* real WPAN device ptr */
|
||||
struct mutex dev_list_mtx; /* mutex for list ops */
|
||||
u16 fragment_tag;
|
||||
};
|
||||
|
||||
static inline struct
|
||||
lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
|
||||
{
|
||||
return netdev_priv(dev);
|
||||
}
|
||||
|
||||
extern struct list_head lowpan_devices;
|
||||
|
||||
int lowpan_frag_rcv(struct sk_buff *skb, const u8 frag_type);
|
||||
void lowpan_net_frag_exit(void);
|
||||
int lowpan_net_frag_init(void);
|
||||
|
||||
void lowpan_rx_init(void);
|
||||
void lowpan_rx_exit(void);
|
||||
|
||||
int lowpan_header_create(struct sk_buff *skb, struct net_device *dev,
|
||||
unsigned short type, const void *_daddr,
|
||||
const void *_saddr, unsigned int len);
|
||||
netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev);
|
||||
|
||||
#endif /* __IEEE802154_6LOWPAN_I_H__ */
|
5
net/ieee802154/6lowpan/Kconfig
Normal file
5
net/ieee802154/6lowpan/Kconfig
Normal file
|
@ -0,0 +1,5 @@
|
|||
config IEEE802154_6LOWPAN
|
||||
tristate "6lowpan support over IEEE 802.15.4"
|
||||
depends on 6LOWPAN
|
||||
---help---
|
||||
IPv6 compression over IEEE 802.15.4.
|
3
net/ieee802154/6lowpan/Makefile
Normal file
3
net/ieee802154/6lowpan/Makefile
Normal file
|
@ -0,0 +1,3 @@
|
|||
obj-$(CONFIG_IEEE802154_6LOWPAN) += ieee802154_6lowpan.o
|
||||
|
||||
ieee802154_6lowpan-y := core.o rx.o reassembly.o tx.o
|
304
net/ieee802154/6lowpan/core.c
Normal file
304
net/ieee802154/6lowpan/core.c
Normal file
|
@ -0,0 +1,304 @@
|
|||
/* Copyright 2011, Siemens AG
|
||||
* written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
|
||||
*/
|
||||
|
||||
/* Based on patches from Jon Smirl <jonsmirl@gmail.com>
|
||||
* Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
/* Jon's code is based on 6lowpan implementation for Contiki which is:
|
||||
* Copyright (c) 2008, Swedish Institute of Computer Science.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* 3. Neither the name of the Institute nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <linux/module.h>
|
||||
#include <linux/netdevice.h>
|
||||
#include <linux/ieee802154.h>
|
||||
|
||||
#include <net/ipv6.h>
|
||||
|
||||
#include "6lowpan_i.h"
|
||||
|
||||
LIST_HEAD(lowpan_devices);
|
||||
static int lowpan_open_count;
|
||||
|
||||
static __le16 lowpan_get_pan_id(const struct net_device *dev)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
|
||||
|
||||
return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
|
||||
}
|
||||
|
||||
static __le16 lowpan_get_short_addr(const struct net_device *dev)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
|
||||
|
||||
return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
|
||||
}
|
||||
|
||||
static u8 lowpan_get_dsn(const struct net_device *dev)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
|
||||
|
||||
return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
|
||||
}
|
||||
|
||||
static struct header_ops lowpan_header_ops = {
|
||||
.create = lowpan_header_create,
|
||||
};
|
||||
|
||||
static struct lock_class_key lowpan_tx_busylock;
|
||||
static struct lock_class_key lowpan_netdev_xmit_lock_key;
|
||||
|
||||
static void lowpan_set_lockdep_class_one(struct net_device *dev,
|
||||
struct netdev_queue *txq,
|
||||
void *_unused)
|
||||
{
|
||||
lockdep_set_class(&txq->_xmit_lock,
|
||||
&lowpan_netdev_xmit_lock_key);
|
||||
}
|
||||
|
||||
static int lowpan_dev_init(struct net_device *dev)
|
||||
{
|
||||
netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL);
|
||||
dev->qdisc_tx_busylock = &lowpan_tx_busylock;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static const struct net_device_ops lowpan_netdev_ops = {
|
||||
.ndo_init = lowpan_dev_init,
|
||||
.ndo_start_xmit = lowpan_xmit,
|
||||
};
|
||||
|
||||
static struct ieee802154_mlme_ops lowpan_mlme = {
|
||||
.get_pan_id = lowpan_get_pan_id,
|
||||
.get_short_addr = lowpan_get_short_addr,
|
||||
.get_dsn = lowpan_get_dsn,
|
||||
};
|
||||
|
||||
static void lowpan_setup(struct net_device *dev)
|
||||
{
|
||||
dev->addr_len = IEEE802154_ADDR_LEN;
|
||||
memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
|
||||
dev->type = ARPHRD_IEEE802154;
|
||||
/* Frame Control + Sequence Number + Address fields + Security Header */
|
||||
dev->hard_header_len = 2 + 1 + 20 + 14;
|
||||
dev->needed_tailroom = 2; /* FCS */
|
||||
dev->mtu = IPV6_MIN_MTU;
|
||||
dev->tx_queue_len = 0;
|
||||
dev->flags = IFF_BROADCAST | IFF_MULTICAST;
|
||||
dev->watchdog_timeo = 0;
|
||||
|
||||
dev->netdev_ops = &lowpan_netdev_ops;
|
||||
dev->header_ops = &lowpan_header_ops;
|
||||
dev->ml_priv = &lowpan_mlme;
|
||||
dev->destructor = free_netdev;
|
||||
}
|
||||
|
||||
static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
|
||||
{
|
||||
if (tb[IFLA_ADDRESS]) {
|
||||
if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
|
||||
return -EINVAL;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lowpan_newlink(struct net *src_net, struct net_device *dev,
|
||||
struct nlattr *tb[], struct nlattr *data[])
|
||||
{
|
||||
struct net_device *real_dev;
|
||||
struct lowpan_dev_record *entry;
|
||||
int ret;
|
||||
|
||||
ASSERT_RTNL();
|
||||
|
||||
pr_debug("adding new link\n");
|
||||
|
||||
if (!tb[IFLA_LINK])
|
||||
return -EINVAL;
|
||||
/* find and hold real wpan device */
|
||||
real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
|
||||
if (!real_dev)
|
||||
return -ENODEV;
|
||||
if (real_dev->type != ARPHRD_IEEE802154) {
|
||||
dev_put(real_dev);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
lowpan_dev_info(dev)->real_dev = real_dev;
|
||||
mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
entry = kzalloc(sizeof(*entry), GFP_KERNEL);
|
||||
if (!entry) {
|
||||
dev_put(real_dev);
|
||||
lowpan_dev_info(dev)->real_dev = NULL;
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
entry->ldev = dev;
|
||||
|
||||
/* Set the lowpan hardware address to the wpan hardware address. */
|
||||
memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);
|
||||
|
||||
mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
INIT_LIST_HEAD(&entry->list);
|
||||
list_add_tail(&entry->list, &lowpan_devices);
|
||||
mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
ret = register_netdevice(dev);
|
||||
if (ret >= 0) {
|
||||
if (!lowpan_open_count)
|
||||
lowpan_rx_init();
|
||||
lowpan_open_count++;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void lowpan_dellink(struct net_device *dev, struct list_head *head)
|
||||
{
|
||||
struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
|
||||
struct net_device *real_dev = lowpan_dev->real_dev;
|
||||
struct lowpan_dev_record *entry, *tmp;
|
||||
|
||||
ASSERT_RTNL();
|
||||
|
||||
lowpan_open_count--;
|
||||
if (!lowpan_open_count)
|
||||
lowpan_rx_exit();
|
||||
|
||||
mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
|
||||
if (entry->ldev == dev) {
|
||||
list_del(&entry->list);
|
||||
kfree(entry);
|
||||
}
|
||||
}
|
||||
mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
unregister_netdevice_queue(dev, head);
|
||||
|
||||
dev_put(real_dev);
|
||||
}
|
||||
|
||||
static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
|
||||
.kind = "lowpan",
|
||||
.priv_size = sizeof(struct lowpan_dev_info),
|
||||
.setup = lowpan_setup,
|
||||
.newlink = lowpan_newlink,
|
||||
.dellink = lowpan_dellink,
|
||||
.validate = lowpan_validate,
|
||||
};
|
||||
|
||||
static inline int __init lowpan_netlink_init(void)
|
||||
{
|
||||
return rtnl_link_register(&lowpan_link_ops);
|
||||
}
|
||||
|
||||
static inline void lowpan_netlink_fini(void)
|
||||
{
|
||||
rtnl_link_unregister(&lowpan_link_ops);
|
||||
}
|
||||
|
||||
static int lowpan_device_event(struct notifier_block *unused,
|
||||
unsigned long event, void *ptr)
|
||||
{
|
||||
struct net_device *dev = netdev_notifier_info_to_dev(ptr);
|
||||
LIST_HEAD(del_list);
|
||||
struct lowpan_dev_record *entry, *tmp;
|
||||
|
||||
if (dev->type != ARPHRD_IEEE802154)
|
||||
goto out;
|
||||
|
||||
if (event == NETDEV_UNREGISTER) {
|
||||
list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
|
||||
if (lowpan_dev_info(entry->ldev)->real_dev == dev)
|
||||
lowpan_dellink(entry->ldev, &del_list);
|
||||
}
|
||||
|
||||
unregister_netdevice_many(&del_list);
|
||||
}
|
||||
|
||||
out:
|
||||
return NOTIFY_DONE;
|
||||
}
|
||||
|
||||
static struct notifier_block lowpan_dev_notifier = {
|
||||
.notifier_call = lowpan_device_event,
|
||||
};
|
||||
|
||||
static int __init lowpan_init_module(void)
|
||||
{
|
||||
int err = 0;
|
||||
|
||||
err = lowpan_net_frag_init();
|
||||
if (err < 0)
|
||||
goto out;
|
||||
|
||||
err = lowpan_netlink_init();
|
||||
if (err < 0)
|
||||
goto out_frag;
|
||||
|
||||
err = register_netdevice_notifier(&lowpan_dev_notifier);
|
||||
if (err < 0)
|
||||
goto out_pack;
|
||||
|
||||
return 0;
|
||||
|
||||
out_pack:
|
||||
lowpan_netlink_fini();
|
||||
out_frag:
|
||||
lowpan_net_frag_exit();
|
||||
out:
|
||||
return err;
|
||||
}
|
||||
|
||||
static void __exit lowpan_cleanup_module(void)
|
||||
{
|
||||
lowpan_netlink_fini();
|
||||
|
||||
lowpan_net_frag_exit();
|
||||
|
||||
unregister_netdevice_notifier(&lowpan_dev_notifier);
|
||||
}
|
||||
|
||||
module_init(lowpan_init_module);
|
||||
module_exit(lowpan_cleanup_module);
|
||||
MODULE_LICENSE("GPL");
|
||||
MODULE_ALIAS_RTNL_LINK("lowpan");
|
|
@ -28,7 +28,7 @@
|
|||
#include <net/ipv6.h>
|
||||
#include <net/inet_frag.h>
|
||||
|
||||
#include "reassembly.h"
|
||||
#include "6lowpan_i.h"
|
||||
|
||||
static const char lowpan_frags_cache_name[] = "lowpan-frags";
|
||||
|
171
net/ieee802154/6lowpan/rx.c
Normal file
171
net/ieee802154/6lowpan/rx.c
Normal file
|
@ -0,0 +1,171 @@
|
|||
/* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include <linux/if_arp.h>
|
||||
|
||||
#include <net/6lowpan.h>
|
||||
#include <net/ieee802154_netdev.h>
|
||||
|
||||
#include "6lowpan_i.h"
|
||||
|
||||
static int lowpan_give_skb_to_devices(struct sk_buff *skb,
|
||||
struct net_device *dev)
|
||||
{
|
||||
struct lowpan_dev_record *entry;
|
||||
struct sk_buff *skb_cp;
|
||||
int stat = NET_RX_SUCCESS;
|
||||
|
||||
skb->protocol = htons(ETH_P_IPV6);
|
||||
skb->pkt_type = PACKET_HOST;
|
||||
|
||||
rcu_read_lock();
|
||||
list_for_each_entry_rcu(entry, &lowpan_devices, list)
|
||||
if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
|
||||
skb_cp = skb_copy(skb, GFP_ATOMIC);
|
||||
if (!skb_cp) {
|
||||
kfree_skb(skb);
|
||||
rcu_read_unlock();
|
||||
return NET_RX_DROP;
|
||||
}
|
||||
|
||||
skb_cp->dev = entry->ldev;
|
||||
stat = netif_rx(skb_cp);
|
||||
if (stat == NET_RX_DROP)
|
||||
break;
|
||||
}
|
||||
rcu_read_unlock();
|
||||
|
||||
consume_skb(skb);
|
||||
|
||||
return stat;
|
||||
}
|
||||
|
||||
static int
|
||||
iphc_decompress(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
|
||||
{
|
||||
u8 iphc0, iphc1;
|
||||
struct ieee802154_addr_sa sa, da;
|
||||
void *sap, *dap;
|
||||
|
||||
raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
|
||||
/* at least two bytes will be used for the encoding */
|
||||
if (skb->len < 2)
|
||||
return -EINVAL;
|
||||
|
||||
if (lowpan_fetch_skb_u8(skb, &iphc0))
|
||||
return -EINVAL;
|
||||
|
||||
if (lowpan_fetch_skb_u8(skb, &iphc1))
|
||||
return -EINVAL;
|
||||
|
||||
ieee802154_addr_to_sa(&sa, &hdr->source);
|
||||
ieee802154_addr_to_sa(&da, &hdr->dest);
|
||||
|
||||
if (sa.addr_type == IEEE802154_ADDR_SHORT)
|
||||
sap = &sa.short_addr;
|
||||
else
|
||||
sap = &sa.hwaddr;
|
||||
|
||||
if (da.addr_type == IEEE802154_ADDR_SHORT)
|
||||
dap = &da.short_addr;
|
||||
else
|
||||
dap = &da.hwaddr;
|
||||
|
||||
return lowpan_header_decompress(skb, skb->dev, sap, sa.addr_type,
|
||||
IEEE802154_ADDR_LEN, dap, da.addr_type,
|
||||
IEEE802154_ADDR_LEN, iphc0, iphc1);
|
||||
}
|
||||
|
||||
static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
|
||||
struct packet_type *pt, struct net_device *orig_dev)
|
||||
{
|
||||
struct ieee802154_hdr hdr;
|
||||
int ret;
|
||||
|
||||
skb = skb_share_check(skb, GFP_ATOMIC);
|
||||
if (!skb)
|
||||
goto drop;
|
||||
|
||||
if (!netif_running(dev))
|
||||
goto drop_skb;
|
||||
|
||||
if (skb->pkt_type == PACKET_OTHERHOST)
|
||||
goto drop_skb;
|
||||
|
||||
if (dev->type != ARPHRD_IEEE802154)
|
||||
goto drop_skb;
|
||||
|
||||
if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
|
||||
goto drop_skb;
|
||||
|
||||
/* check that it's our buffer */
|
||||
if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
|
||||
/* Pull off the 1-byte of 6lowpan header. */
|
||||
skb_pull(skb, 1);
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
} else {
|
||||
switch (skb->data[0] & 0xe0) {
|
||||
case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
|
||||
ret = iphc_decompress(skb, &hdr);
|
||||
if (ret < 0)
|
||||
goto drop_skb;
|
||||
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
|
||||
ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
|
||||
if (ret == 1) {
|
||||
ret = iphc_decompress(skb, &hdr);
|
||||
if (ret < 0)
|
||||
goto drop_skb;
|
||||
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
} else if (ret == -1) {
|
||||
return NET_RX_DROP;
|
||||
} else {
|
||||
return NET_RX_SUCCESS;
|
||||
}
|
||||
case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
|
||||
ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
|
||||
if (ret == 1) {
|
||||
ret = iphc_decompress(skb, &hdr);
|
||||
if (ret < 0)
|
||||
goto drop_skb;
|
||||
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
} else if (ret == -1) {
|
||||
return NET_RX_DROP;
|
||||
} else {
|
||||
return NET_RX_SUCCESS;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
drop_skb:
|
||||
kfree_skb(skb);
|
||||
drop:
|
||||
return NET_RX_DROP;
|
||||
}
|
||||
|
||||
static struct packet_type lowpan_packet_type = {
|
||||
.type = htons(ETH_P_IEEE802154),
|
||||
.func = lowpan_rcv,
|
||||
};
|
||||
|
||||
void lowpan_rx_init(void)
|
||||
{
|
||||
dev_add_pack(&lowpan_packet_type);
|
||||
}
|
||||
|
||||
void lowpan_rx_exit(void)
|
||||
{
|
||||
dev_remove_pack(&lowpan_packet_type);
|
||||
}
|
271
net/ieee802154/6lowpan/tx.c
Normal file
271
net/ieee802154/6lowpan/tx.c
Normal file
|
@ -0,0 +1,271 @@
|
|||
/* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
#include <net/6lowpan.h>
|
||||
#include <net/ieee802154_netdev.h>
|
||||
|
||||
#include "6lowpan_i.h"
|
||||
|
||||
/* don't save pan id, it's intra pan */
|
||||
struct lowpan_addr {
|
||||
u8 mode;
|
||||
union {
|
||||
/* IPv6 needs big endian here */
|
||||
__be64 extended_addr;
|
||||
__be16 short_addr;
|
||||
} u;
|
||||
};
|
||||
|
||||
struct lowpan_addr_info {
|
||||
struct lowpan_addr daddr;
|
||||
struct lowpan_addr saddr;
|
||||
};
|
||||
|
||||
static inline struct
|
||||
lowpan_addr_info *lowpan_skb_priv(const struct sk_buff *skb)
|
||||
{
|
||||
WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct lowpan_addr_info));
|
||||
return (struct lowpan_addr_info *)(skb->data -
|
||||
sizeof(struct lowpan_addr_info));
|
||||
}
|
||||
|
||||
int lowpan_header_create(struct sk_buff *skb, struct net_device *dev,
|
||||
unsigned short type, const void *_daddr,
|
||||
const void *_saddr, unsigned int len)
|
||||
{
|
||||
const u8 *saddr = _saddr;
|
||||
const u8 *daddr = _daddr;
|
||||
struct lowpan_addr_info *info;
|
||||
|
||||
/* TODO:
|
||||
* if this package isn't ipv6 one, where should it be routed?
|
||||
*/
|
||||
if (type != ETH_P_IPV6)
|
||||
return 0;
|
||||
|
||||
if (!saddr)
|
||||
saddr = dev->dev_addr;
|
||||
|
||||
raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
|
||||
raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
|
||||
|
||||
info = lowpan_skb_priv(skb);
|
||||
|
||||
/* TODO: Currently we only support extended_addr */
|
||||
info->daddr.mode = IEEE802154_ADDR_LONG;
|
||||
memcpy(&info->daddr.u.extended_addr, daddr,
|
||||
sizeof(info->daddr.u.extended_addr));
|
||||
info->saddr.mode = IEEE802154_ADDR_LONG;
|
||||
memcpy(&info->saddr.u.extended_addr, saddr,
|
||||
sizeof(info->daddr.u.extended_addr));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct sk_buff*
|
||||
lowpan_alloc_frag(struct sk_buff *skb, int size,
|
||||
const struct ieee802154_hdr *master_hdr)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev;
|
||||
struct sk_buff *frag;
|
||||
int rc;
|
||||
|
||||
frag = alloc_skb(real_dev->hard_header_len +
|
||||
real_dev->needed_tailroom + size,
|
||||
GFP_ATOMIC);
|
||||
|
||||
if (likely(frag)) {
|
||||
frag->dev = real_dev;
|
||||
frag->priority = skb->priority;
|
||||
skb_reserve(frag, real_dev->hard_header_len);
|
||||
skb_reset_network_header(frag);
|
||||
*mac_cb(frag) = *mac_cb(skb);
|
||||
|
||||
rc = dev_hard_header(frag, real_dev, 0, &master_hdr->dest,
|
||||
&master_hdr->source, size);
|
||||
if (rc < 0) {
|
||||
kfree_skb(frag);
|
||||
return ERR_PTR(rc);
|
||||
}
|
||||
} else {
|
||||
frag = ERR_PTR(-ENOMEM);
|
||||
}
|
||||
|
||||
return frag;
|
||||
}
|
||||
|
||||
static int
|
||||
lowpan_xmit_fragment(struct sk_buff *skb, const struct ieee802154_hdr *wpan_hdr,
|
||||
u8 *frag_hdr, int frag_hdrlen,
|
||||
int offset, int len)
|
||||
{
|
||||
struct sk_buff *frag;
|
||||
|
||||
raw_dump_inline(__func__, " fragment header", frag_hdr, frag_hdrlen);
|
||||
|
||||
frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
|
||||
if (IS_ERR(frag))
|
||||
return -PTR_ERR(frag);
|
||||
|
||||
memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
|
||||
memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
|
||||
|
||||
raw_dump_table(__func__, " fragment dump", frag->data, frag->len);
|
||||
|
||||
return dev_queue_xmit(frag);
|
||||
}
|
||||
|
||||
static int
|
||||
lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev,
|
||||
const struct ieee802154_hdr *wpan_hdr)
|
||||
{
|
||||
u16 dgram_size, dgram_offset;
|
||||
__be16 frag_tag;
|
||||
u8 frag_hdr[5];
|
||||
int frag_cap, frag_len, payload_cap, rc;
|
||||
int skb_unprocessed, skb_offset;
|
||||
|
||||
dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
|
||||
skb->mac_len;
|
||||
frag_tag = htons(lowpan_dev_info(dev)->fragment_tag);
|
||||
lowpan_dev_info(dev)->fragment_tag++;
|
||||
|
||||
frag_hdr[0] = LOWPAN_DISPATCH_FRAG1 | ((dgram_size >> 8) & 0x07);
|
||||
frag_hdr[1] = dgram_size & 0xff;
|
||||
memcpy(frag_hdr + 2, &frag_tag, sizeof(frag_tag));
|
||||
|
||||
payload_cap = ieee802154_max_payload(wpan_hdr);
|
||||
|
||||
frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
|
||||
skb_network_header_len(skb), 8);
|
||||
|
||||
skb_offset = skb_network_header_len(skb);
|
||||
skb_unprocessed = skb->len - skb->mac_len - skb_offset;
|
||||
|
||||
rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
|
||||
LOWPAN_FRAG1_HEAD_SIZE, 0,
|
||||
frag_len + skb_network_header_len(skb));
|
||||
if (rc) {
|
||||
pr_debug("%s unable to send FRAG1 packet (tag: %d)",
|
||||
__func__, ntohs(frag_tag));
|
||||
goto err;
|
||||
}
|
||||
|
||||
frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1;
|
||||
frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN;
|
||||
frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8);
|
||||
|
||||
do {
|
||||
dgram_offset += frag_len;
|
||||
skb_offset += frag_len;
|
||||
skb_unprocessed -= frag_len;
|
||||
frag_len = min(frag_cap, skb_unprocessed);
|
||||
|
||||
frag_hdr[4] = dgram_offset >> 3;
|
||||
|
||||
rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
|
||||
LOWPAN_FRAGN_HEAD_SIZE, skb_offset,
|
||||
frag_len);
|
||||
if (rc) {
|
||||
pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
|
||||
__func__, ntohs(frag_tag), skb_offset);
|
||||
goto err;
|
||||
}
|
||||
} while (skb_unprocessed > frag_cap);
|
||||
|
||||
consume_skb(skb);
|
||||
return NET_XMIT_SUCCESS;
|
||||
|
||||
err:
|
||||
kfree_skb(skb);
|
||||
return rc;
|
||||
}
|
||||
|
||||
static int lowpan_header(struct sk_buff *skb, struct net_device *dev)
|
||||
{
|
||||
struct ieee802154_addr sa, da;
|
||||
struct ieee802154_mac_cb *cb = mac_cb_init(skb);
|
||||
struct lowpan_addr_info info;
|
||||
void *daddr, *saddr;
|
||||
|
||||
memcpy(&info, lowpan_skb_priv(skb), sizeof(info));
|
||||
|
||||
/* TODO: Currently we only support extended_addr */
|
||||
daddr = &info.daddr.u.extended_addr;
|
||||
saddr = &info.saddr.u.extended_addr;
|
||||
|
||||
lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len);
|
||||
|
||||
cb->type = IEEE802154_FC_TYPE_DATA;
|
||||
|
||||
/* prepare wpan address data */
|
||||
sa.mode = IEEE802154_ADDR_LONG;
|
||||
sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
|
||||
sa.extended_addr = ieee802154_devaddr_from_raw(saddr);
|
||||
|
||||
/* intra-PAN communications */
|
||||
da.pan_id = sa.pan_id;
|
||||
|
||||
/* if the destination address is the broadcast address, use the
|
||||
* corresponding short address
|
||||
*/
|
||||
if (lowpan_is_addr_broadcast((const u8 *)daddr)) {
|
||||
da.mode = IEEE802154_ADDR_SHORT;
|
||||
da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
|
||||
cb->ackreq = false;
|
||||
} else {
|
||||
da.mode = IEEE802154_ADDR_LONG;
|
||||
da.extended_addr = ieee802154_devaddr_from_raw(daddr);
|
||||
cb->ackreq = true;
|
||||
}
|
||||
|
||||
return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
|
||||
ETH_P_IPV6, (void *)&da, (void *)&sa, 0);
|
||||
}
|
||||
|
||||
netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
|
||||
{
|
||||
struct ieee802154_hdr wpan_hdr;
|
||||
int max_single, ret;
|
||||
|
||||
pr_debug("package xmit\n");
|
||||
|
||||
/* We must take a copy of the skb before we modify/replace the ipv6
|
||||
* header as the header could be used elsewhere
|
||||
*/
|
||||
skb = skb_unshare(skb, GFP_ATOMIC);
|
||||
if (!skb)
|
||||
return NET_XMIT_DROP;
|
||||
|
||||
ret = lowpan_header(skb, dev);
|
||||
if (ret < 0) {
|
||||
kfree_skb(skb);
|
||||
return NET_XMIT_DROP;
|
||||
}
|
||||
|
||||
if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) {
|
||||
kfree_skb(skb);
|
||||
return NET_XMIT_DROP;
|
||||
}
|
||||
|
||||
max_single = ieee802154_max_payload(&wpan_hdr);
|
||||
|
||||
if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) {
|
||||
skb->dev = lowpan_dev_info(dev)->real_dev;
|
||||
return dev_queue_xmit(skb);
|
||||
} else {
|
||||
netdev_tx_t rc;
|
||||
|
||||
pr_debug("frame is too big, fragmentation is needed\n");
|
||||
rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr);
|
||||
|
||||
return rc < 0 ? NET_XMIT_DROP : rc;
|
||||
}
|
||||
}
|
|
@ -1,729 +0,0 @@
|
|||
/* Copyright 2011, Siemens AG
|
||||
* written by Alexander Smirnov <alex.bluesman.smirnov@gmail.com>
|
||||
*/
|
||||
|
||||
/* Based on patches from Jon Smirl <jonsmirl@gmail.com>
|
||||
* Copyright (c) 2011 Jon Smirl <jonsmirl@gmail.com>
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*/
|
||||
|
||||
/* Jon's code is based on 6lowpan implementation for Contiki which is:
|
||||
* Copyright (c) 2008, Swedish Institute of Computer Science.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* 3. Neither the name of the Institute nor the names of its contributors
|
||||
* may be used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
|
||||
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
|
||||
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
|
||||
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
|
||||
* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||||
* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
|
||||
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
|
||||
* SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <linux/bitops.h>
|
||||
#include <linux/if_arp.h>
|
||||
#include <linux/module.h>
|
||||
#include <linux/moduleparam.h>
|
||||
#include <linux/netdevice.h>
|
||||
#include <linux/ieee802154.h>
|
||||
#include <net/af_ieee802154.h>
|
||||
#include <net/ieee802154_netdev.h>
|
||||
#include <net/6lowpan.h>
|
||||
#include <net/ipv6.h>
|
||||
|
||||
#include "reassembly.h"
|
||||
|
||||
static LIST_HEAD(lowpan_devices);
|
||||
static int lowpan_open_count;
|
||||
|
||||
/* private device info */
|
||||
struct lowpan_dev_info {
|
||||
struct net_device *real_dev; /* real WPAN device ptr */
|
||||
struct mutex dev_list_mtx; /* mutex for list ops */
|
||||
u16 fragment_tag;
|
||||
};
|
||||
|
||||
struct lowpan_dev_record {
|
||||
struct net_device *ldev;
|
||||
struct list_head list;
|
||||
};
|
||||
|
||||
/* don't save pan id, it's intra pan */
|
||||
struct lowpan_addr {
|
||||
u8 mode;
|
||||
union {
|
||||
/* IPv6 needs big endian here */
|
||||
__be64 extended_addr;
|
||||
__be16 short_addr;
|
||||
} u;
|
||||
};
|
||||
|
||||
struct lowpan_addr_info {
|
||||
struct lowpan_addr daddr;
|
||||
struct lowpan_addr saddr;
|
||||
};
|
||||
|
||||
static inline struct
|
||||
lowpan_dev_info *lowpan_dev_info(const struct net_device *dev)
|
||||
{
|
||||
return netdev_priv(dev);
|
||||
}
|
||||
|
||||
static inline struct
|
||||
lowpan_addr_info *lowpan_skb_priv(const struct sk_buff *skb)
|
||||
{
|
||||
WARN_ON_ONCE(skb_headroom(skb) < sizeof(struct lowpan_addr_info));
|
||||
return (struct lowpan_addr_info *)(skb->data -
|
||||
sizeof(struct lowpan_addr_info));
|
||||
}
|
||||
|
||||
static int lowpan_header_create(struct sk_buff *skb, struct net_device *dev,
|
||||
unsigned short type, const void *_daddr,
|
||||
const void *_saddr, unsigned int len)
|
||||
{
|
||||
const u8 *saddr = _saddr;
|
||||
const u8 *daddr = _daddr;
|
||||
struct lowpan_addr_info *info;
|
||||
|
||||
/* TODO:
|
||||
* if this package isn't ipv6 one, where should it be routed?
|
||||
*/
|
||||
if (type != ETH_P_IPV6)
|
||||
return 0;
|
||||
|
||||
if (!saddr)
|
||||
saddr = dev->dev_addr;
|
||||
|
||||
raw_dump_inline(__func__, "saddr", (unsigned char *)saddr, 8);
|
||||
raw_dump_inline(__func__, "daddr", (unsigned char *)daddr, 8);
|
||||
|
||||
info = lowpan_skb_priv(skb);
|
||||
|
||||
/* TODO: Currently we only support extended_addr */
|
||||
info->daddr.mode = IEEE802154_ADDR_LONG;
|
||||
memcpy(&info->daddr.u.extended_addr, daddr,
|
||||
sizeof(info->daddr.u.extended_addr));
|
||||
info->saddr.mode = IEEE802154_ADDR_LONG;
|
||||
memcpy(&info->saddr.u.extended_addr, saddr,
|
||||
sizeof(info->daddr.u.extended_addr));
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lowpan_give_skb_to_devices(struct sk_buff *skb,
|
||||
struct net_device *dev)
|
||||
{
|
||||
struct lowpan_dev_record *entry;
|
||||
struct sk_buff *skb_cp;
|
||||
int stat = NET_RX_SUCCESS;
|
||||
|
||||
skb->protocol = htons(ETH_P_IPV6);
|
||||
skb->pkt_type = PACKET_HOST;
|
||||
|
||||
rcu_read_lock();
|
||||
list_for_each_entry_rcu(entry, &lowpan_devices, list)
|
||||
if (lowpan_dev_info(entry->ldev)->real_dev == skb->dev) {
|
||||
skb_cp = skb_copy(skb, GFP_ATOMIC);
|
||||
if (!skb_cp) {
|
||||
kfree_skb(skb);
|
||||
rcu_read_unlock();
|
||||
return NET_RX_DROP;
|
||||
}
|
||||
|
||||
skb_cp->dev = entry->ldev;
|
||||
stat = netif_rx(skb_cp);
|
||||
if (stat == NET_RX_DROP)
|
||||
break;
|
||||
}
|
||||
rcu_read_unlock();
|
||||
|
||||
consume_skb(skb);
|
||||
|
||||
return stat;
|
||||
}
|
||||
|
||||
static int
|
||||
iphc_decompress(struct sk_buff *skb, const struct ieee802154_hdr *hdr)
|
||||
{
|
||||
u8 iphc0, iphc1;
|
||||
struct ieee802154_addr_sa sa, da;
|
||||
void *sap, *dap;
|
||||
|
||||
raw_dump_table(__func__, "raw skb data dump", skb->data, skb->len);
|
||||
/* at least two bytes will be used for the encoding */
|
||||
if (skb->len < 2)
|
||||
return -EINVAL;
|
||||
|
||||
if (lowpan_fetch_skb_u8(skb, &iphc0))
|
||||
return -EINVAL;
|
||||
|
||||
if (lowpan_fetch_skb_u8(skb, &iphc1))
|
||||
return -EINVAL;
|
||||
|
||||
ieee802154_addr_to_sa(&sa, &hdr->source);
|
||||
ieee802154_addr_to_sa(&da, &hdr->dest);
|
||||
|
||||
if (sa.addr_type == IEEE802154_ADDR_SHORT)
|
||||
sap = &sa.short_addr;
|
||||
else
|
||||
sap = &sa.hwaddr;
|
||||
|
||||
if (da.addr_type == IEEE802154_ADDR_SHORT)
|
||||
dap = &da.short_addr;
|
||||
else
|
||||
dap = &da.hwaddr;
|
||||
|
||||
return lowpan_header_decompress(skb, skb->dev, sap, sa.addr_type,
|
||||
IEEE802154_ADDR_LEN, dap, da.addr_type,
|
||||
IEEE802154_ADDR_LEN, iphc0, iphc1);
|
||||
}
|
||||
|
||||
static struct sk_buff*
|
||||
lowpan_alloc_frag(struct sk_buff *skb, int size,
|
||||
const struct ieee802154_hdr *master_hdr)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(skb->dev)->real_dev;
|
||||
struct sk_buff *frag;
|
||||
int rc;
|
||||
|
||||
frag = alloc_skb(real_dev->hard_header_len +
|
||||
real_dev->needed_tailroom + size,
|
||||
GFP_ATOMIC);
|
||||
|
||||
if (likely(frag)) {
|
||||
frag->dev = real_dev;
|
||||
frag->priority = skb->priority;
|
||||
skb_reserve(frag, real_dev->hard_header_len);
|
||||
skb_reset_network_header(frag);
|
||||
*mac_cb(frag) = *mac_cb(skb);
|
||||
|
||||
rc = dev_hard_header(frag, real_dev, 0, &master_hdr->dest,
|
||||
&master_hdr->source, size);
|
||||
if (rc < 0) {
|
||||
kfree_skb(frag);
|
||||
return ERR_PTR(rc);
|
||||
}
|
||||
} else {
|
||||
frag = ERR_PTR(-ENOMEM);
|
||||
}
|
||||
|
||||
return frag;
|
||||
}
|
||||
|
||||
static int
|
||||
lowpan_xmit_fragment(struct sk_buff *skb, const struct ieee802154_hdr *wpan_hdr,
|
||||
u8 *frag_hdr, int frag_hdrlen,
|
||||
int offset, int len)
|
||||
{
|
||||
struct sk_buff *frag;
|
||||
|
||||
raw_dump_inline(__func__, " fragment header", frag_hdr, frag_hdrlen);
|
||||
|
||||
frag = lowpan_alloc_frag(skb, frag_hdrlen + len, wpan_hdr);
|
||||
if (IS_ERR(frag))
|
||||
return -PTR_ERR(frag);
|
||||
|
||||
memcpy(skb_put(frag, frag_hdrlen), frag_hdr, frag_hdrlen);
|
||||
memcpy(skb_put(frag, len), skb_network_header(skb) + offset, len);
|
||||
|
||||
raw_dump_table(__func__, " fragment dump", frag->data, frag->len);
|
||||
|
||||
return dev_queue_xmit(frag);
|
||||
}
|
||||
|
||||
static int
|
||||
lowpan_xmit_fragmented(struct sk_buff *skb, struct net_device *dev,
|
||||
const struct ieee802154_hdr *wpan_hdr)
|
||||
{
|
||||
u16 dgram_size, dgram_offset;
|
||||
__be16 frag_tag;
|
||||
u8 frag_hdr[5];
|
||||
int frag_cap, frag_len, payload_cap, rc;
|
||||
int skb_unprocessed, skb_offset;
|
||||
|
||||
dgram_size = lowpan_uncompress_size(skb, &dgram_offset) -
|
||||
skb->mac_len;
|
||||
frag_tag = htons(lowpan_dev_info(dev)->fragment_tag);
|
||||
lowpan_dev_info(dev)->fragment_tag++;
|
||||
|
||||
frag_hdr[0] = LOWPAN_DISPATCH_FRAG1 | ((dgram_size >> 8) & 0x07);
|
||||
frag_hdr[1] = dgram_size & 0xff;
|
||||
memcpy(frag_hdr + 2, &frag_tag, sizeof(frag_tag));
|
||||
|
||||
payload_cap = ieee802154_max_payload(wpan_hdr);
|
||||
|
||||
frag_len = round_down(payload_cap - LOWPAN_FRAG1_HEAD_SIZE -
|
||||
skb_network_header_len(skb), 8);
|
||||
|
||||
skb_offset = skb_network_header_len(skb);
|
||||
skb_unprocessed = skb->len - skb->mac_len - skb_offset;
|
||||
|
||||
rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
|
||||
LOWPAN_FRAG1_HEAD_SIZE, 0,
|
||||
frag_len + skb_network_header_len(skb));
|
||||
if (rc) {
|
||||
pr_debug("%s unable to send FRAG1 packet (tag: %d)",
|
||||
__func__, ntohs(frag_tag));
|
||||
goto err;
|
||||
}
|
||||
|
||||
frag_hdr[0] &= ~LOWPAN_DISPATCH_FRAG1;
|
||||
frag_hdr[0] |= LOWPAN_DISPATCH_FRAGN;
|
||||
frag_cap = round_down(payload_cap - LOWPAN_FRAGN_HEAD_SIZE, 8);
|
||||
|
||||
do {
|
||||
dgram_offset += frag_len;
|
||||
skb_offset += frag_len;
|
||||
skb_unprocessed -= frag_len;
|
||||
frag_len = min(frag_cap, skb_unprocessed);
|
||||
|
||||
frag_hdr[4] = dgram_offset >> 3;
|
||||
|
||||
rc = lowpan_xmit_fragment(skb, wpan_hdr, frag_hdr,
|
||||
LOWPAN_FRAGN_HEAD_SIZE, skb_offset,
|
||||
frag_len);
|
||||
if (rc) {
|
||||
pr_debug("%s unable to send a FRAGN packet. (tag: %d, offset: %d)\n",
|
||||
__func__, ntohs(frag_tag), skb_offset);
|
||||
goto err;
|
||||
}
|
||||
} while (skb_unprocessed > frag_cap);
|
||||
|
||||
consume_skb(skb);
|
||||
return NET_XMIT_SUCCESS;
|
||||
|
||||
err:
|
||||
kfree_skb(skb);
|
||||
return rc;
|
||||
}
|
||||
|
||||
static int lowpan_header(struct sk_buff *skb, struct net_device *dev)
|
||||
{
|
||||
struct ieee802154_addr sa, da;
|
||||
struct ieee802154_mac_cb *cb = mac_cb_init(skb);
|
||||
struct lowpan_addr_info info;
|
||||
void *daddr, *saddr;
|
||||
|
||||
memcpy(&info, lowpan_skb_priv(skb), sizeof(info));
|
||||
|
||||
/* TODO: Currently we only support extended_addr */
|
||||
daddr = &info.daddr.u.extended_addr;
|
||||
saddr = &info.saddr.u.extended_addr;
|
||||
|
||||
lowpan_header_compress(skb, dev, ETH_P_IPV6, daddr, saddr, skb->len);
|
||||
|
||||
cb->type = IEEE802154_FC_TYPE_DATA;
|
||||
|
||||
/* prepare wpan address data */
|
||||
sa.mode = IEEE802154_ADDR_LONG;
|
||||
sa.pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
|
||||
sa.extended_addr = ieee802154_devaddr_from_raw(saddr);
|
||||
|
||||
/* intra-PAN communications */
|
||||
da.pan_id = sa.pan_id;
|
||||
|
||||
/* if the destination address is the broadcast address, use the
|
||||
* corresponding short address
|
||||
*/
|
||||
if (lowpan_is_addr_broadcast((const u8 *)daddr)) {
|
||||
da.mode = IEEE802154_ADDR_SHORT;
|
||||
da.short_addr = cpu_to_le16(IEEE802154_ADDR_BROADCAST);
|
||||
cb->ackreq = false;
|
||||
} else {
|
||||
da.mode = IEEE802154_ADDR_LONG;
|
||||
da.extended_addr = ieee802154_devaddr_from_raw(daddr);
|
||||
cb->ackreq = true;
|
||||
}
|
||||
|
||||
return dev_hard_header(skb, lowpan_dev_info(dev)->real_dev,
|
||||
ETH_P_IPV6, (void *)&da, (void *)&sa, 0);
|
||||
}
|
||||
|
||||
static netdev_tx_t lowpan_xmit(struct sk_buff *skb, struct net_device *dev)
|
||||
{
|
||||
struct ieee802154_hdr wpan_hdr;
|
||||
int max_single, ret;
|
||||
|
||||
pr_debug("package xmit\n");
|
||||
|
||||
/* We must take a copy of the skb before we modify/replace the ipv6
|
||||
* header as the header could be used elsewhere
|
||||
*/
|
||||
skb = skb_unshare(skb, GFP_ATOMIC);
|
||||
if (!skb)
|
||||
return NET_XMIT_DROP;
|
||||
|
||||
ret = lowpan_header(skb, dev);
|
||||
if (ret < 0) {
|
||||
kfree_skb(skb);
|
||||
return NET_XMIT_DROP;
|
||||
}
|
||||
|
||||
if (ieee802154_hdr_peek(skb, &wpan_hdr) < 0) {
|
||||
kfree_skb(skb);
|
||||
return NET_XMIT_DROP;
|
||||
}
|
||||
|
||||
max_single = ieee802154_max_payload(&wpan_hdr);
|
||||
|
||||
if (skb_tail_pointer(skb) - skb_network_header(skb) <= max_single) {
|
||||
skb->dev = lowpan_dev_info(dev)->real_dev;
|
||||
return dev_queue_xmit(skb);
|
||||
} else {
|
||||
netdev_tx_t rc;
|
||||
|
||||
pr_debug("frame is too big, fragmentation is needed\n");
|
||||
rc = lowpan_xmit_fragmented(skb, dev, &wpan_hdr);
|
||||
|
||||
return rc < 0 ? NET_XMIT_DROP : rc;
|
||||
}
|
||||
}
|
||||
|
||||
static __le16 lowpan_get_pan_id(const struct net_device *dev)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
|
||||
|
||||
return ieee802154_mlme_ops(real_dev)->get_pan_id(real_dev);
|
||||
}
|
||||
|
||||
static __le16 lowpan_get_short_addr(const struct net_device *dev)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
|
||||
|
||||
return ieee802154_mlme_ops(real_dev)->get_short_addr(real_dev);
|
||||
}
|
||||
|
||||
static u8 lowpan_get_dsn(const struct net_device *dev)
|
||||
{
|
||||
struct net_device *real_dev = lowpan_dev_info(dev)->real_dev;
|
||||
|
||||
return ieee802154_mlme_ops(real_dev)->get_dsn(real_dev);
|
||||
}
|
||||
|
||||
static struct header_ops lowpan_header_ops = {
|
||||
.create = lowpan_header_create,
|
||||
};
|
||||
|
||||
static struct lock_class_key lowpan_tx_busylock;
|
||||
static struct lock_class_key lowpan_netdev_xmit_lock_key;
|
||||
|
||||
static void lowpan_set_lockdep_class_one(struct net_device *dev,
|
||||
struct netdev_queue *txq,
|
||||
void *_unused)
|
||||
{
|
||||
lockdep_set_class(&txq->_xmit_lock,
|
||||
&lowpan_netdev_xmit_lock_key);
|
||||
}
|
||||
|
||||
static int lowpan_dev_init(struct net_device *dev)
|
||||
{
|
||||
netdev_for_each_tx_queue(dev, lowpan_set_lockdep_class_one, NULL);
|
||||
dev->qdisc_tx_busylock = &lowpan_tx_busylock;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static const struct net_device_ops lowpan_netdev_ops = {
|
||||
.ndo_init = lowpan_dev_init,
|
||||
.ndo_start_xmit = lowpan_xmit,
|
||||
};
|
||||
|
||||
static struct ieee802154_mlme_ops lowpan_mlme = {
|
||||
.get_pan_id = lowpan_get_pan_id,
|
||||
.get_short_addr = lowpan_get_short_addr,
|
||||
.get_dsn = lowpan_get_dsn,
|
||||
};
|
||||
|
||||
static void lowpan_setup(struct net_device *dev)
|
||||
{
|
||||
dev->addr_len = IEEE802154_ADDR_LEN;
|
||||
memset(dev->broadcast, 0xff, IEEE802154_ADDR_LEN);
|
||||
dev->type = ARPHRD_IEEE802154;
|
||||
/* Frame Control + Sequence Number + Address fields + Security Header */
|
||||
dev->hard_header_len = 2 + 1 + 20 + 14;
|
||||
dev->needed_tailroom = 2; /* FCS */
|
||||
dev->mtu = IPV6_MIN_MTU;
|
||||
dev->tx_queue_len = 0;
|
||||
dev->flags = IFF_BROADCAST | IFF_MULTICAST;
|
||||
dev->watchdog_timeo = 0;
|
||||
|
||||
dev->netdev_ops = &lowpan_netdev_ops;
|
||||
dev->header_ops = &lowpan_header_ops;
|
||||
dev->ml_priv = &lowpan_mlme;
|
||||
dev->destructor = free_netdev;
|
||||
}
|
||||
|
||||
static int lowpan_validate(struct nlattr *tb[], struct nlattr *data[])
|
||||
{
|
||||
if (tb[IFLA_ADDRESS]) {
|
||||
if (nla_len(tb[IFLA_ADDRESS]) != IEEE802154_ADDR_LEN)
|
||||
return -EINVAL;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int lowpan_rcv(struct sk_buff *skb, struct net_device *dev,
|
||||
struct packet_type *pt, struct net_device *orig_dev)
|
||||
{
|
||||
struct ieee802154_hdr hdr;
|
||||
int ret;
|
||||
|
||||
skb = skb_share_check(skb, GFP_ATOMIC);
|
||||
if (!skb)
|
||||
goto drop;
|
||||
|
||||
if (!netif_running(dev))
|
||||
goto drop_skb;
|
||||
|
||||
if (skb->pkt_type == PACKET_OTHERHOST)
|
||||
goto drop_skb;
|
||||
|
||||
if (dev->type != ARPHRD_IEEE802154)
|
||||
goto drop_skb;
|
||||
|
||||
if (ieee802154_hdr_peek_addrs(skb, &hdr) < 0)
|
||||
goto drop_skb;
|
||||
|
||||
/* check that it's our buffer */
|
||||
if (skb->data[0] == LOWPAN_DISPATCH_IPV6) {
|
||||
/* Pull off the 1-byte of 6lowpan header. */
|
||||
skb_pull(skb, 1);
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
} else {
|
||||
switch (skb->data[0] & 0xe0) {
|
||||
case LOWPAN_DISPATCH_IPHC: /* ipv6 datagram */
|
||||
ret = iphc_decompress(skb, &hdr);
|
||||
if (ret < 0)
|
||||
goto drop_skb;
|
||||
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
case LOWPAN_DISPATCH_FRAG1: /* first fragment header */
|
||||
ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAG1);
|
||||
if (ret == 1) {
|
||||
ret = iphc_decompress(skb, &hdr);
|
||||
if (ret < 0)
|
||||
goto drop_skb;
|
||||
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
} else if (ret == -1) {
|
||||
return NET_RX_DROP;
|
||||
} else {
|
||||
return NET_RX_SUCCESS;
|
||||
}
|
||||
case LOWPAN_DISPATCH_FRAGN: /* next fragments headers */
|
||||
ret = lowpan_frag_rcv(skb, LOWPAN_DISPATCH_FRAGN);
|
||||
if (ret == 1) {
|
||||
ret = iphc_decompress(skb, &hdr);
|
||||
if (ret < 0)
|
||||
goto drop_skb;
|
||||
|
||||
return lowpan_give_skb_to_devices(skb, NULL);
|
||||
} else if (ret == -1) {
|
||||
return NET_RX_DROP;
|
||||
} else {
|
||||
return NET_RX_SUCCESS;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
drop_skb:
|
||||
kfree_skb(skb);
|
||||
drop:
|
||||
return NET_RX_DROP;
|
||||
}
|
||||
|
||||
static struct packet_type lowpan_packet_type = {
|
||||
.type = htons(ETH_P_IEEE802154),
|
||||
.func = lowpan_rcv,
|
||||
};
|
||||
|
||||
static int lowpan_newlink(struct net *src_net, struct net_device *dev,
|
||||
struct nlattr *tb[], struct nlattr *data[])
|
||||
{
|
||||
struct net_device *real_dev;
|
||||
struct lowpan_dev_record *entry;
|
||||
int ret;
|
||||
|
||||
ASSERT_RTNL();
|
||||
|
||||
pr_debug("adding new link\n");
|
||||
|
||||
if (!tb[IFLA_LINK])
|
||||
return -EINVAL;
|
||||
/* find and hold real wpan device */
|
||||
real_dev = dev_get_by_index(src_net, nla_get_u32(tb[IFLA_LINK]));
|
||||
if (!real_dev)
|
||||
return -ENODEV;
|
||||
if (real_dev->type != ARPHRD_IEEE802154) {
|
||||
dev_put(real_dev);
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
lowpan_dev_info(dev)->real_dev = real_dev;
|
||||
mutex_init(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
entry = kzalloc(sizeof(*entry), GFP_KERNEL);
|
||||
if (!entry) {
|
||||
dev_put(real_dev);
|
||||
lowpan_dev_info(dev)->real_dev = NULL;
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
entry->ldev = dev;
|
||||
|
||||
/* Set the lowpan hardware address to the wpan hardware address. */
|
||||
memcpy(dev->dev_addr, real_dev->dev_addr, IEEE802154_ADDR_LEN);
|
||||
|
||||
mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
INIT_LIST_HEAD(&entry->list);
|
||||
list_add_tail(&entry->list, &lowpan_devices);
|
||||
mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
ret = register_netdevice(dev);
|
||||
if (ret >= 0) {
|
||||
if (!lowpan_open_count)
|
||||
dev_add_pack(&lowpan_packet_type);
|
||||
lowpan_open_count++;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static void lowpan_dellink(struct net_device *dev, struct list_head *head)
|
||||
{
|
||||
struct lowpan_dev_info *lowpan_dev = lowpan_dev_info(dev);
|
||||
struct net_device *real_dev = lowpan_dev->real_dev;
|
||||
struct lowpan_dev_record *entry, *tmp;
|
||||
|
||||
ASSERT_RTNL();
|
||||
|
||||
lowpan_open_count--;
|
||||
if (!lowpan_open_count)
|
||||
dev_remove_pack(&lowpan_packet_type);
|
||||
|
||||
mutex_lock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
|
||||
if (entry->ldev == dev) {
|
||||
list_del(&entry->list);
|
||||
kfree(entry);
|
||||
}
|
||||
}
|
||||
mutex_unlock(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
mutex_destroy(&lowpan_dev_info(dev)->dev_list_mtx);
|
||||
|
||||
unregister_netdevice_queue(dev, head);
|
||||
|
||||
dev_put(real_dev);
|
||||
}
|
||||
|
||||
static struct rtnl_link_ops lowpan_link_ops __read_mostly = {
|
||||
.kind = "lowpan",
|
||||
.priv_size = sizeof(struct lowpan_dev_info),
|
||||
.setup = lowpan_setup,
|
||||
.newlink = lowpan_newlink,
|
||||
.dellink = lowpan_dellink,
|
||||
.validate = lowpan_validate,
|
||||
};
|
||||
|
||||
static inline int __init lowpan_netlink_init(void)
|
||||
{
|
||||
return rtnl_link_register(&lowpan_link_ops);
|
||||
}
|
||||
|
||||
static inline void lowpan_netlink_fini(void)
|
||||
{
|
||||
rtnl_link_unregister(&lowpan_link_ops);
|
||||
}
|
||||
|
||||
static int lowpan_device_event(struct notifier_block *unused,
|
||||
unsigned long event, void *ptr)
|
||||
{
|
||||
struct net_device *dev = netdev_notifier_info_to_dev(ptr);
|
||||
LIST_HEAD(del_list);
|
||||
struct lowpan_dev_record *entry, *tmp;
|
||||
|
||||
if (dev->type != ARPHRD_IEEE802154)
|
||||
goto out;
|
||||
|
||||
if (event == NETDEV_UNREGISTER) {
|
||||
list_for_each_entry_safe(entry, tmp, &lowpan_devices, list) {
|
||||
if (lowpan_dev_info(entry->ldev)->real_dev == dev)
|
||||
lowpan_dellink(entry->ldev, &del_list);
|
||||
}
|
||||
|
||||
unregister_netdevice_many(&del_list);
|
||||
}
|
||||
|
||||
out:
|
||||
return NOTIFY_DONE;
|
||||
}
|
||||
|
||||
static struct notifier_block lowpan_dev_notifier = {
|
||||
.notifier_call = lowpan_device_event,
|
||||
};
|
||||
|
||||
static int __init lowpan_init_module(void)
|
||||
{
|
||||
int err = 0;
|
||||
|
||||
err = lowpan_net_frag_init();
|
||||
if (err < 0)
|
||||
goto out;
|
||||
|
||||
err = lowpan_netlink_init();
|
||||
if (err < 0)
|
||||
goto out_frag;
|
||||
|
||||
err = register_netdevice_notifier(&lowpan_dev_notifier);
|
||||
if (err < 0)
|
||||
goto out_pack;
|
||||
|
||||
return 0;
|
||||
|
||||
out_pack:
|
||||
lowpan_netlink_fini();
|
||||
out_frag:
|
||||
lowpan_net_frag_exit();
|
||||
out:
|
||||
return err;
|
||||
}
|
||||
|
||||
static void __exit lowpan_cleanup_module(void)
|
||||
{
|
||||
lowpan_netlink_fini();
|
||||
|
||||
lowpan_net_frag_exit();
|
||||
|
||||
unregister_netdevice_notifier(&lowpan_dev_notifier);
|
||||
}
|
||||
|
||||
module_init(lowpan_init_module);
|
||||
module_exit(lowpan_cleanup_module);
|
||||
MODULE_LICENSE("GPL");
|
||||
MODULE_ALIAS_RTNL_LINK("lowpan");
|
|
@ -1,4 +1,4 @@
|
|||
config IEEE802154
|
||||
menuconfig IEEE802154
|
||||
tristate "IEEE Std 802.15.4 Low-Rate Wireless Personal Area Networks support"
|
||||
---help---
|
||||
IEEE Std 802.15.4 defines a low data rate, low power and low
|
||||
|
@ -10,8 +10,16 @@ config IEEE802154
|
|||
Say Y here to compile LR-WPAN support into the kernel or say M to
|
||||
compile it as modules.
|
||||
|
||||
config IEEE802154_6LOWPAN
|
||||
tristate "6lowpan support over IEEE 802.15.4"
|
||||
depends on IEEE802154 && 6LOWPAN
|
||||
if IEEE802154
|
||||
|
||||
config IEEE802154_SOCKET
|
||||
tristate "IEEE 802.15.4 socket interface"
|
||||
default y
|
||||
---help---
|
||||
IPv6 compression over IEEE 802.15.4.
|
||||
Socket interface for IEEE 802.15.4. Contains DGRAM sockets interface
|
||||
for 802.15.4 dataframes. Also RAW socket interface to build MAC
|
||||
header from userspace.
|
||||
|
||||
source "net/ieee802154/6lowpan/Kconfig"
|
||||
|
||||
endif
|
||||
|
|
|
@ -1,9 +1,9 @@
|
|||
obj-$(CONFIG_IEEE802154) += ieee802154.o af_802154.o
|
||||
obj-$(CONFIG_IEEE802154_6LOWPAN) += ieee802154_6lowpan.o
|
||||
obj-$(CONFIG_IEEE802154) += ieee802154.o
|
||||
obj-$(CONFIG_IEEE802154_SOCKET) += ieee802154_socket.o
|
||||
obj-y += 6lowpan/
|
||||
|
||||
ieee802154_6lowpan-y := 6lowpan_rtnl.o reassembly.o
|
||||
ieee802154-y := netlink.o nl-mac.o nl-phy.o nl_policy.o core.o \
|
||||
header_ops.o sysfs.o nl802154.o
|
||||
af_802154-y := af_ieee802154.o raw.o dgram.o
|
||||
ieee802154_socket-y := socket.o
|
||||
|
||||
ccflags-y += -D__CHECK_ENDIAN__
|
||||
|
|
|
@ -1,33 +0,0 @@
|
|||
/*
|
||||
* Internal interfaces for ieee 802.15.4 address family.
|
||||
*
|
||||
* Copyright 2007, 2008, 2009 Siemens AG
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* Written by:
|
||||
* Sergey Lapin <slapin@ossfans.org>
|
||||
* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
|
||||
*/
|
||||
|
||||
#ifndef AF802154_H
|
||||
#define AF802154_H
|
||||
|
||||
struct sk_buff;
|
||||
struct net_device;
|
||||
struct ieee802154_addr;
|
||||
extern struct proto ieee802154_raw_prot;
|
||||
extern struct proto ieee802154_dgram_prot;
|
||||
void ieee802154_raw_deliver(struct net_device *dev, struct sk_buff *skb);
|
||||
int ieee802154_dgram_deliver(struct net_device *dev, struct sk_buff *skb);
|
||||
struct net_device *ieee802154_get_dev(struct net *net,
|
||||
const struct ieee802154_addr *addr);
|
||||
|
||||
#endif
|
|
@ -1,369 +0,0 @@
|
|||
/*
|
||||
* IEEE802154.4 socket interface
|
||||
*
|
||||
* Copyright 2007, 2008 Siemens AG
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* Written by:
|
||||
* Sergey Lapin <slapin@ossfans.org>
|
||||
* Maxim Gorbachyov <maxim.gorbachev@siemens.com>
|
||||
*/
|
||||
|
||||
#include <linux/net.h>
|
||||
#include <linux/capability.h>
|
||||
#include <linux/module.h>
|
||||
#include <linux/if_arp.h>
|
||||
#include <linux/if.h>
|
||||
#include <linux/termios.h> /* For TIOCOUTQ/INQ */
|
||||
#include <linux/list.h>
|
||||
#include <linux/slab.h>
|
||||
#include <net/datalink.h>
|
||||
#include <net/psnap.h>
|
||||
#include <net/sock.h>
|
||||
#include <net/tcp_states.h>
|
||||
#include <net/route.h>
|
||||
|
||||
#include <net/af_ieee802154.h>
|
||||
#include <net/ieee802154_netdev.h>
|
||||
|
||||
#include "af802154.h"
|
||||
|
||||
/* Utility function for families */
|
||||
struct net_device*
|
||||
ieee802154_get_dev(struct net *net, const struct ieee802154_addr *addr)
|
||||
{
|
||||
struct net_device *dev = NULL;
|
||||
struct net_device *tmp;
|
||||
__le16 pan_id, short_addr;
|
||||
u8 hwaddr[IEEE802154_ADDR_LEN];
|
||||
|
||||
switch (addr->mode) {
|
||||
case IEEE802154_ADDR_LONG:
|
||||
ieee802154_devaddr_to_raw(hwaddr, addr->extended_addr);
|
||||
rcu_read_lock();
|
||||
dev = dev_getbyhwaddr_rcu(net, ARPHRD_IEEE802154, hwaddr);
|
||||
if (dev)
|
||||
dev_hold(dev);
|
||||
rcu_read_unlock();
|
||||
break;
|
||||
case IEEE802154_ADDR_SHORT:
|
||||
if (addr->pan_id == cpu_to_le16(IEEE802154_PANID_BROADCAST) ||
|
||||
addr->short_addr == cpu_to_le16(IEEE802154_ADDR_UNDEF) ||
|
||||
addr->short_addr == cpu_to_le16(IEEE802154_ADDR_BROADCAST))
|
||||
break;
|
||||
|
||||
rtnl_lock();
|
||||
|
||||
for_each_netdev(net, tmp) {
|
||||
if (tmp->type != ARPHRD_IEEE802154)
|
||||
continue;
|
||||
|
||||
pan_id = ieee802154_mlme_ops(tmp)->get_pan_id(tmp);
|
||||
short_addr =
|
||||
ieee802154_mlme_ops(tmp)->get_short_addr(tmp);
|
||||
|
||||
if (pan_id == addr->pan_id &&
|
||||
short_addr == addr->short_addr) {
|
||||
dev = tmp;
|
||||
dev_hold(dev);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
rtnl_unlock();
|
||||
break;
|
||||
default:
|
||||
pr_warn("Unsupported ieee802154 address type: %d\n",
|
||||
addr->mode);
|
||||
break;
|
||||
}
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
static int ieee802154_sock_release(struct socket *sock)
|
||||
{
|
||||
struct sock *sk = sock->sk;
|
||||
|
||||
if (sk) {
|
||||
sock->sk = NULL;
|
||||
sk->sk_prot->close(sk, 0);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int ieee802154_sock_sendmsg(struct kiocb *iocb, struct socket *sock,
|
||||
struct msghdr *msg, size_t len)
|
||||
{
|
||||
struct sock *sk = sock->sk;
|
||||
|
||||
return sk->sk_prot->sendmsg(iocb, sk, msg, len);
|
||||
}
|
||||
|
||||
static int ieee802154_sock_bind(struct socket *sock, struct sockaddr *uaddr,
|
||||
int addr_len)
|
||||
{
|
||||
struct sock *sk = sock->sk;
|
||||
|
||||
if (sk->sk_prot->bind)
|
||||
return sk->sk_prot->bind(sk, uaddr, addr_len);
|
||||
|
||||
return sock_no_bind(sock, uaddr, addr_len);
|
||||
}
|
||||
|
||||
static int ieee802154_sock_connect(struct socket *sock, struct sockaddr *uaddr,
|
||||
int addr_len, int flags)
|
||||
{
|
||||
struct sock *sk = sock->sk;
|
||||
|
||||
if (addr_len < sizeof(uaddr->sa_family))
|
||||
return -EINVAL;
|
||||
|
||||
if (uaddr->sa_family == AF_UNSPEC)
|
||||
return sk->sk_prot->disconnect(sk, flags);
|
||||
|
||||
return sk->sk_prot->connect(sk, uaddr, addr_len);
|
||||
}
|
||||
|
||||
static int ieee802154_dev_ioctl(struct sock *sk, struct ifreq __user *arg,
|
||||
unsigned int cmd)
|
||||
{
|
||||
struct ifreq ifr;
|
||||
int ret = -ENOIOCTLCMD;
|
||||
struct net_device *dev;
|
||||
|
||||
if (copy_from_user(&ifr, arg, sizeof(struct ifreq)))
|
||||
return -EFAULT;
|
||||
|
||||
ifr.ifr_name[IFNAMSIZ-1] = 0;
|
||||
|
||||
dev_load(sock_net(sk), ifr.ifr_name);
|
||||
dev = dev_get_by_name(sock_net(sk), ifr.ifr_name);
|
||||
|
||||
if (!dev)
|
||||
return -ENODEV;
|
||||
|
||||
if (dev->type == ARPHRD_IEEE802154 && dev->netdev_ops->ndo_do_ioctl)
|
||||
ret = dev->netdev_ops->ndo_do_ioctl(dev, &ifr, cmd);
|
||||
|
||||
if (!ret && copy_to_user(arg, &ifr, sizeof(struct ifreq)))
|
||||
ret = -EFAULT;
|
||||
dev_put(dev);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int ieee802154_sock_ioctl(struct socket *sock, unsigned int cmd,
|
||||
unsigned long arg)
|
||||
{
|
||||
struct sock *sk = sock->sk;
|
||||
|
||||
switch (cmd) {
|
||||
case SIOCGSTAMP:
|
||||
return sock_get_timestamp(sk, (struct timeval __user *)arg);
|
||||
case SIOCGSTAMPNS:
|
||||
return sock_get_timestampns(sk, (struct timespec __user *)arg);
|
||||
case SIOCGIFADDR:
|
||||
case SIOCSIFADDR:
|
||||
return ieee802154_dev_ioctl(sk, (struct ifreq __user *)arg,
|
||||
cmd);
|
||||
default:
|
||||
if (!sk->sk_prot->ioctl)
|
||||
return -ENOIOCTLCMD;
|
||||
return sk->sk_prot->ioctl(sk, cmd, arg);
|
||||
}
|
||||
}
|
||||
|
||||
static const struct proto_ops ieee802154_raw_ops = {
|
||||
.family = PF_IEEE802154,
|
||||
.owner = THIS_MODULE,
|
||||
.release = ieee802154_sock_release,
|
||||
.bind = ieee802154_sock_bind,
|
||||
.connect = ieee802154_sock_connect,
|
||||
.socketpair = sock_no_socketpair,
|
||||
.accept = sock_no_accept,
|
||||
.getname = sock_no_getname,
|
||||
.poll = datagram_poll,
|
||||
.ioctl = ieee802154_sock_ioctl,
|
||||
.listen = sock_no_listen,
|
||||
.shutdown = sock_no_shutdown,
|
||||
.setsockopt = sock_common_setsockopt,
|
||||
.getsockopt = sock_common_getsockopt,
|
||||
.sendmsg = ieee802154_sock_sendmsg,
|
||||
.recvmsg = sock_common_recvmsg,
|
||||
.mmap = sock_no_mmap,
|
||||
.sendpage = sock_no_sendpage,
|
||||
#ifdef CONFIG_COMPAT
|
||||
.compat_setsockopt = compat_sock_common_setsockopt,
|
||||
.compat_getsockopt = compat_sock_common_getsockopt,
|
||||
#endif
|
||||
};
|
||||
|
||||
static const struct proto_ops ieee802154_dgram_ops = {
|
||||
.family = PF_IEEE802154,
|
||||
.owner = THIS_MODULE,
|
||||
.release = ieee802154_sock_release,
|
||||
.bind = ieee802154_sock_bind,
|
||||
.connect = ieee802154_sock_connect,
|
||||
.socketpair = sock_no_socketpair,
|
||||
.accept = sock_no_accept,
|
||||
.getname = sock_no_getname,
|
||||
.poll = datagram_poll,
|
||||
.ioctl = ieee802154_sock_ioctl,
|
||||
.listen = sock_no_listen,
|
||||
.shutdown = sock_no_shutdown,
|
||||
.setsockopt = sock_common_setsockopt,
|
||||
.getsockopt = sock_common_getsockopt,
|
||||
.sendmsg = ieee802154_sock_sendmsg,
|
||||
.recvmsg = sock_common_recvmsg,
|
||||
.mmap = sock_no_mmap,
|
||||
.sendpage = sock_no_sendpage,
|
||||
#ifdef CONFIG_COMPAT
|
||||
.compat_setsockopt = compat_sock_common_setsockopt,
|
||||
.compat_getsockopt = compat_sock_common_getsockopt,
|
||||
#endif
|
||||
};
|
||||
|
||||
/* Create a socket. Initialise the socket, blank the addresses
|
||||
* set the state.
|
||||
*/
|
||||
static int ieee802154_create(struct net *net, struct socket *sock,
|
||||
int protocol, int kern)
|
||||
{
|
||||
struct sock *sk;
|
||||
int rc;
|
||||
struct proto *proto;
|
||||
const struct proto_ops *ops;
|
||||
|
||||
if (!net_eq(net, &init_net))
|
||||
return -EAFNOSUPPORT;
|
||||
|
||||
switch (sock->type) {
|
||||
case SOCK_RAW:
|
||||
proto = &ieee802154_raw_prot;
|
||||
ops = &ieee802154_raw_ops;
|
||||
break;
|
||||
case SOCK_DGRAM:
|
||||
proto = &ieee802154_dgram_prot;
|
||||
ops = &ieee802154_dgram_ops;
|
||||
break;
|
||||
default:
|
||||
rc = -ESOCKTNOSUPPORT;
|
||||
goto out;
|
||||
}
|
||||
|
||||
rc = -ENOMEM;
|
||||
sk = sk_alloc(net, PF_IEEE802154, GFP_KERNEL, proto);
|
||||
if (!sk)
|
||||
goto out;
|
||||
rc = 0;
|
||||
|
||||
sock->ops = ops;
|
||||
|
||||
sock_init_data(sock, sk);
|
||||
/* FIXME: sk->sk_destruct */
|
||||
sk->sk_family = PF_IEEE802154;
|
||||
|
||||
/* Checksums on by default */
|
||||
sock_set_flag(sk, SOCK_ZAPPED);
|
||||
|
||||
if (sk->sk_prot->hash)
|
||||
sk->sk_prot->hash(sk);
|
||||
|
||||
if (sk->sk_prot->init) {
|
||||
rc = sk->sk_prot->init(sk);
|
||||
if (rc)
|
||||
sk_common_release(sk);
|
||||
}
|
||||
out:
|
||||
return rc;
|
||||
}
|
||||
|
||||
static const struct net_proto_family ieee802154_family_ops = {
|
||||
.family = PF_IEEE802154,
|
||||
.create = ieee802154_create,
|
||||
.owner = THIS_MODULE,
|
||||
};
|
||||
|
||||
static int ieee802154_rcv(struct sk_buff *skb, struct net_device *dev,
|
||||
struct packet_type *pt, struct net_device *orig_dev)
|
||||
{
|
||||
if (!netif_running(dev))
|
||||
goto drop;
|
||||
pr_debug("got frame, type %d, dev %p\n", dev->type, dev);
|
||||
#ifdef DEBUG
|
||||
print_hex_dump_bytes("ieee802154_rcv ",
|
||||
DUMP_PREFIX_NONE, skb->data, skb->len);
|
||||
#endif
|
||||
|
||||
if (!net_eq(dev_net(dev), &init_net))
|
||||
goto drop;
|
||||
|
||||
ieee802154_raw_deliver(dev, skb);
|
||||
|
||||
if (dev->type != ARPHRD_IEEE802154)
|
||||
goto drop;
|
||||
|
||||
if (skb->pkt_type != PACKET_OTHERHOST)
|
||||
return ieee802154_dgram_deliver(dev, skb);
|
||||
|
||||
drop:
|
||||
kfree_skb(skb);
|
||||
return NET_RX_DROP;
|
||||
}
|
||||
|
||||
static struct packet_type ieee802154_packet_type = {
|
||||
.type = htons(ETH_P_IEEE802154),
|
||||
.func = ieee802154_rcv,
|
||||
};
|
||||
|
||||
static int __init af_ieee802154_init(void)
|
||||
{
|
||||
int rc = -EINVAL;
|
||||
|
||||
rc = proto_register(&ieee802154_raw_prot, 1);
|
||||
if (rc)
|
||||
goto out;
|
||||
|
||||
rc = proto_register(&ieee802154_dgram_prot, 1);
|
||||
if (rc)
|
||||
goto err_dgram;
|
||||
|
||||
/* Tell SOCKET that we are alive */
|
||||
rc = sock_register(&ieee802154_family_ops);
|
||||
if (rc)
|
||||
goto err_sock;
|
||||
dev_add_pack(&ieee802154_packet_type);
|
||||
|
||||
rc = 0;
|
||||
goto out;
|
||||
|
||||
err_sock:
|
||||
proto_unregister(&ieee802154_dgram_prot);
|
||||
err_dgram:
|
||||
proto_unregister(&ieee802154_raw_prot);
|
||||
out:
|
||||
return rc;
|
||||
}
|
||||
|
||||
static void __exit af_ieee802154_remove(void)
|
||||
{
|
||||
dev_remove_pack(&ieee802154_packet_type);
|
||||
sock_unregister(PF_IEEE802154);
|
||||
proto_unregister(&ieee802154_dgram_prot);
|
||||
proto_unregister(&ieee802154_raw_prot);
|
||||
}
|
||||
|
||||
module_init(af_ieee802154_init);
|
||||
module_exit(af_ieee802154_remove);
|
||||
|
||||
MODULE_LICENSE("GPL");
|
||||
MODULE_ALIAS_NETPROTO(PF_IEEE802154);
|
|
@ -1,549 +0,0 @@
|
|||
/*
|
||||
* IEEE 802.15.4 dgram socket interface
|
||||
*
|
||||
* Copyright 2007, 2008 Siemens AG
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* Written by:
|
||||
* Sergey Lapin <slapin@ossfans.org>
|
||||
* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
|
||||
*/
|
||||
|
||||
#include <linux/capability.h>
|
||||
#include <linux/net.h>
|
||||
#include <linux/module.h>
|
||||
#include <linux/if_arp.h>
|
||||
#include <linux/list.h>
|
||||
#include <linux/slab.h>
|
||||
#include <linux/ieee802154.h>
|
||||
#include <net/sock.h>
|
||||
#include <net/af_ieee802154.h>
|
||||
#include <net/ieee802154_netdev.h>
|
||||
|
||||
#include <asm/ioctls.h>
|
||||
|
||||
#include "af802154.h"
|
||||
|
||||
static HLIST_HEAD(dgram_head);
|
||||
static DEFINE_RWLOCK(dgram_lock);
|
||||
|
||||
struct dgram_sock {
|
||||
struct sock sk;
|
||||
|
||||
struct ieee802154_addr src_addr;
|
||||
struct ieee802154_addr dst_addr;
|
||||
|
||||
unsigned int bound:1;
|
||||
unsigned int connected:1;
|
||||
unsigned int want_ack:1;
|
||||
unsigned int secen:1;
|
||||
unsigned int secen_override:1;
|
||||
unsigned int seclevel:3;
|
||||
unsigned int seclevel_override:1;
|
||||
};
|
||||
|
||||
static inline struct dgram_sock *dgram_sk(const struct sock *sk)
|
||||
{
|
||||
return container_of(sk, struct dgram_sock, sk);
|
||||
}
|
||||
|
||||
static void dgram_hash(struct sock *sk)
|
||||
{
|
||||
write_lock_bh(&dgram_lock);
|
||||
sk_add_node(sk, &dgram_head);
|
||||
sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
|
||||
write_unlock_bh(&dgram_lock);
|
||||
}
|
||||
|
||||
static void dgram_unhash(struct sock *sk)
|
||||
{
|
||||
write_lock_bh(&dgram_lock);
|
||||
if (sk_del_node_init(sk))
|
||||
sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
|
||||
write_unlock_bh(&dgram_lock);
|
||||
}
|
||||
|
||||
static int dgram_init(struct sock *sk)
|
||||
{
|
||||
struct dgram_sock *ro = dgram_sk(sk);
|
||||
|
||||
ro->want_ack = 1;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static void dgram_close(struct sock *sk, long timeout)
|
||||
{
|
||||
sk_common_release(sk);
|
||||
}
|
||||
|
||||
static int dgram_bind(struct sock *sk, struct sockaddr *uaddr, int len)
|
||||
{
|
||||
struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
|
||||
struct ieee802154_addr haddr;
|
||||
struct dgram_sock *ro = dgram_sk(sk);
|
||||
int err = -EINVAL;
|
||||
struct net_device *dev;
|
||||
|
||||
lock_sock(sk);
|
||||
|
||||
ro->bound = 0;
|
||||
|
||||
if (len < sizeof(*addr))
|
||||
goto out;
|
||||
|
||||
if (addr->family != AF_IEEE802154)
|
||||
goto out;
|
||||
|
||||
ieee802154_addr_from_sa(&haddr, &addr->addr);
|
||||
dev = ieee802154_get_dev(sock_net(sk), &haddr);
|
||||
if (!dev) {
|
||||
err = -ENODEV;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (dev->type != ARPHRD_IEEE802154) {
|
||||
err = -ENODEV;
|
||||
goto out_put;
|
||||
}
|
||||
|
||||
ro->src_addr = haddr;
|
||||
|
||||
ro->bound = 1;
|
||||
err = 0;
|
||||
out_put:
|
||||
dev_put(dev);
|
||||
out:
|
||||
release_sock(sk);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
static int dgram_ioctl(struct sock *sk, int cmd, unsigned long arg)
|
||||
{
|
||||
switch (cmd) {
|
||||
case SIOCOUTQ:
|
||||
{
|
||||
int amount = sk_wmem_alloc_get(sk);
|
||||
|
||||
return put_user(amount, (int __user *)arg);
|
||||
}
|
||||
|
||||
case SIOCINQ:
|
||||
{
|
||||
struct sk_buff *skb;
|
||||
unsigned long amount;
|
||||
|
||||
amount = 0;
|
||||
spin_lock_bh(&sk->sk_receive_queue.lock);
|
||||
skb = skb_peek(&sk->sk_receive_queue);
|
||||
if (skb != NULL) {
|
||||
/* We will only return the amount
|
||||
* of this packet since that is all
|
||||
* that will be read.
|
||||
*/
|
||||
amount = skb->len - ieee802154_hdr_length(skb);
|
||||
}
|
||||
spin_unlock_bh(&sk->sk_receive_queue.lock);
|
||||
return put_user(amount, (int __user *)arg);
|
||||
}
|
||||
}
|
||||
|
||||
return -ENOIOCTLCMD;
|
||||
}
|
||||
|
||||
/* FIXME: autobind */
|
||||
static int dgram_connect(struct sock *sk, struct sockaddr *uaddr,
|
||||
int len)
|
||||
{
|
||||
struct sockaddr_ieee802154 *addr = (struct sockaddr_ieee802154 *)uaddr;
|
||||
struct dgram_sock *ro = dgram_sk(sk);
|
||||
int err = 0;
|
||||
|
||||
if (len < sizeof(*addr))
|
||||
return -EINVAL;
|
||||
|
||||
if (addr->family != AF_IEEE802154)
|
||||
return -EINVAL;
|
||||
|
||||
lock_sock(sk);
|
||||
|
||||
if (!ro->bound) {
|
||||
err = -ENETUNREACH;
|
||||
goto out;
|
||||
}
|
||||
|
||||
ieee802154_addr_from_sa(&ro->dst_addr, &addr->addr);
|
||||
ro->connected = 1;
|
||||
|
||||
out:
|
||||
release_sock(sk);
|
||||
return err;
|
||||
}
|
||||
|
||||
static int dgram_disconnect(struct sock *sk, int flags)
|
||||
{
|
||||
struct dgram_sock *ro = dgram_sk(sk);
|
||||
|
||||
lock_sock(sk);
|
||||
ro->connected = 0;
|
||||
release_sock(sk);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int dgram_sendmsg(struct kiocb *iocb, struct sock *sk,
|
||||
struct msghdr *msg, size_t size)
|
||||
{
|
||||
struct net_device *dev;
|
||||
unsigned int mtu;
|
||||
struct sk_buff *skb;
|
||||
struct ieee802154_mac_cb *cb;
|
||||
struct dgram_sock *ro = dgram_sk(sk);
|
||||
struct ieee802154_addr dst_addr;
|
||||
int hlen, tlen;
|
||||
int err;
|
||||
|
||||
if (msg->msg_flags & MSG_OOB) {
|
||||
pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
|
||||
return -EOPNOTSUPP;
|
||||
}
|
||||
|
||||
if (!ro->connected && !msg->msg_name)
|
||||
return -EDESTADDRREQ;
|
||||
else if (ro->connected && msg->msg_name)
|
||||
return -EISCONN;
|
||||
|
||||
if (!ro->bound)
|
||||
dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
|
||||
else
|
||||
dev = ieee802154_get_dev(sock_net(sk), &ro->src_addr);
|
||||
|
||||
if (!dev) {
|
||||
pr_debug("no dev\n");
|
||||
err = -ENXIO;
|
||||
goto out;
|
||||
}
|
||||
mtu = dev->mtu;
|
||||
pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
|
||||
|
||||
if (size > mtu) {
|
||||
pr_debug("size = %Zu, mtu = %u\n", size, mtu);
|
||||
err = -EMSGSIZE;
|
||||
goto out_dev;
|
||||
}
|
||||
|
||||
hlen = LL_RESERVED_SPACE(dev);
|
||||
tlen = dev->needed_tailroom;
|
||||
skb = sock_alloc_send_skb(sk, hlen + tlen + size,
|
||||
msg->msg_flags & MSG_DONTWAIT,
|
||||
&err);
|
||||
if (!skb)
|
||||
goto out_dev;
|
||||
|
||||
skb_reserve(skb, hlen);
|
||||
|
||||
skb_reset_network_header(skb);
|
||||
|
||||
cb = mac_cb_init(skb);
|
||||
cb->type = IEEE802154_FC_TYPE_DATA;
|
||||
cb->ackreq = ro->want_ack;
|
||||
|
||||
if (msg->msg_name) {
|
||||
DECLARE_SOCKADDR(struct sockaddr_ieee802154*,
|
||||
daddr, msg->msg_name);
|
||||
|
||||
ieee802154_addr_from_sa(&dst_addr, &daddr->addr);
|
||||
} else {
|
||||
dst_addr = ro->dst_addr;
|
||||
}
|
||||
|
||||
cb->secen = ro->secen;
|
||||
cb->secen_override = ro->secen_override;
|
||||
cb->seclevel = ro->seclevel;
|
||||
cb->seclevel_override = ro->seclevel_override;
|
||||
|
||||
err = dev_hard_header(skb, dev, ETH_P_IEEE802154, &dst_addr,
|
||||
ro->bound ? &ro->src_addr : NULL, size);
|
||||
if (err < 0)
|
||||
goto out_skb;
|
||||
|
||||
err = memcpy_from_msg(skb_put(skb, size), msg, size);
|
||||
if (err < 0)
|
||||
goto out_skb;
|
||||
|
||||
skb->dev = dev;
|
||||
skb->sk = sk;
|
||||
skb->protocol = htons(ETH_P_IEEE802154);
|
||||
|
||||
dev_put(dev);
|
||||
|
||||
err = dev_queue_xmit(skb);
|
||||
if (err > 0)
|
||||
err = net_xmit_errno(err);
|
||||
|
||||
return err ?: size;
|
||||
|
||||
out_skb:
|
||||
kfree_skb(skb);
|
||||
out_dev:
|
||||
dev_put(dev);
|
||||
out:
|
||||
return err;
|
||||
}
|
||||
|
||||
static int dgram_recvmsg(struct kiocb *iocb, struct sock *sk,
|
||||
struct msghdr *msg, size_t len, int noblock,
|
||||
int flags, int *addr_len)
|
||||
{
|
||||
size_t copied = 0;
|
||||
int err = -EOPNOTSUPP;
|
||||
struct sk_buff *skb;
|
||||
DECLARE_SOCKADDR(struct sockaddr_ieee802154 *, saddr, msg->msg_name);
|
||||
|
||||
skb = skb_recv_datagram(sk, flags, noblock, &err);
|
||||
if (!skb)
|
||||
goto out;
|
||||
|
||||
copied = skb->len;
|
||||
if (len < copied) {
|
||||
msg->msg_flags |= MSG_TRUNC;
|
||||
copied = len;
|
||||
}
|
||||
|
||||
/* FIXME: skip headers if necessary ?! */
|
||||
err = skb_copy_datagram_msg(skb, 0, msg, copied);
|
||||
if (err)
|
||||
goto done;
|
||||
|
||||
sock_recv_ts_and_drops(msg, sk, skb);
|
||||
|
||||
if (saddr) {
|
||||
saddr->family = AF_IEEE802154;
|
||||
ieee802154_addr_to_sa(&saddr->addr, &mac_cb(skb)->source);
|
||||
*addr_len = sizeof(*saddr);
|
||||
}
|
||||
|
||||
if (flags & MSG_TRUNC)
|
||||
copied = skb->len;
|
||||
done:
|
||||
skb_free_datagram(sk, skb);
|
||||
out:
|
||||
if (err)
|
||||
return err;
|
||||
return copied;
|
||||
}
|
||||
|
||||
static int dgram_rcv_skb(struct sock *sk, struct sk_buff *skb)
|
||||
{
|
||||
skb = skb_share_check(skb, GFP_ATOMIC);
|
||||
if (!skb)
|
||||
return NET_RX_DROP;
|
||||
|
||||
if (sock_queue_rcv_skb(sk, skb) < 0) {
|
||||
kfree_skb(skb);
|
||||
return NET_RX_DROP;
|
||||
}
|
||||
|
||||
return NET_RX_SUCCESS;
|
||||
}
|
||||
|
||||
static inline bool
|
||||
ieee802154_match_sock(__le64 hw_addr, __le16 pan_id, __le16 short_addr,
|
||||
struct dgram_sock *ro)
|
||||
{
|
||||
if (!ro->bound)
|
||||
return true;
|
||||
|
||||
if (ro->src_addr.mode == IEEE802154_ADDR_LONG &&
|
||||
hw_addr == ro->src_addr.extended_addr)
|
||||
return true;
|
||||
|
||||
if (ro->src_addr.mode == IEEE802154_ADDR_SHORT &&
|
||||
pan_id == ro->src_addr.pan_id &&
|
||||
short_addr == ro->src_addr.short_addr)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int ieee802154_dgram_deliver(struct net_device *dev, struct sk_buff *skb)
|
||||
{
|
||||
struct sock *sk, *prev = NULL;
|
||||
int ret = NET_RX_SUCCESS;
|
||||
__le16 pan_id, short_addr;
|
||||
__le64 hw_addr;
|
||||
|
||||
/* Data frame processing */
|
||||
BUG_ON(dev->type != ARPHRD_IEEE802154);
|
||||
|
||||
pan_id = ieee802154_mlme_ops(dev)->get_pan_id(dev);
|
||||
short_addr = ieee802154_mlme_ops(dev)->get_short_addr(dev);
|
||||
hw_addr = ieee802154_devaddr_from_raw(dev->dev_addr);
|
||||
|
||||
read_lock(&dgram_lock);
|
||||
sk_for_each(sk, &dgram_head) {
|
||||
if (ieee802154_match_sock(hw_addr, pan_id, short_addr,
|
||||
dgram_sk(sk))) {
|
||||
if (prev) {
|
||||
struct sk_buff *clone;
|
||||
|
||||
clone = skb_clone(skb, GFP_ATOMIC);
|
||||
if (clone)
|
||||
dgram_rcv_skb(prev, clone);
|
||||
}
|
||||
|
||||
prev = sk;
|
||||
}
|
||||
}
|
||||
|
||||
if (prev) {
|
||||
dgram_rcv_skb(prev, skb);
|
||||
} else {
|
||||
kfree_skb(skb);
|
||||
ret = NET_RX_DROP;
|
||||
}
|
||||
read_unlock(&dgram_lock);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
static int dgram_getsockopt(struct sock *sk, int level, int optname,
|
||||
char __user *optval, int __user *optlen)
|
||||
{
|
||||
struct dgram_sock *ro = dgram_sk(sk);
|
||||
|
||||
int val, len;
|
||||
|
||||
if (level != SOL_IEEE802154)
|
||||
return -EOPNOTSUPP;
|
||||
|
||||
if (get_user(len, optlen))
|
||||
return -EFAULT;
|
||||
|
||||
len = min_t(unsigned int, len, sizeof(int));
|
||||
|
||||
switch (optname) {
|
||||
case WPAN_WANTACK:
|
||||
val = ro->want_ack;
|
||||
break;
|
||||
case WPAN_SECURITY:
|
||||
if (!ro->secen_override)
|
||||
val = WPAN_SECURITY_DEFAULT;
|
||||
else if (ro->secen)
|
||||
val = WPAN_SECURITY_ON;
|
||||
else
|
||||
val = WPAN_SECURITY_OFF;
|
||||
break;
|
||||
case WPAN_SECURITY_LEVEL:
|
||||
if (!ro->seclevel_override)
|
||||
val = WPAN_SECURITY_LEVEL_DEFAULT;
|
||||
else
|
||||
val = ro->seclevel;
|
||||
break;
|
||||
default:
|
||||
return -ENOPROTOOPT;
|
||||
}
|
||||
|
||||
if (put_user(len, optlen))
|
||||
return -EFAULT;
|
||||
if (copy_to_user(optval, &val, len))
|
||||
return -EFAULT;
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int dgram_setsockopt(struct sock *sk, int level, int optname,
|
||||
char __user *optval, unsigned int optlen)
|
||||
{
|
||||
struct dgram_sock *ro = dgram_sk(sk);
|
||||
struct net *net = sock_net(sk);
|
||||
int val;
|
||||
int err = 0;
|
||||
|
||||
if (optlen < sizeof(int))
|
||||
return -EINVAL;
|
||||
|
||||
if (get_user(val, (int __user *)optval))
|
||||
return -EFAULT;
|
||||
|
||||
lock_sock(sk);
|
||||
|
||||
switch (optname) {
|
||||
case WPAN_WANTACK:
|
||||
ro->want_ack = !!val;
|
||||
break;
|
||||
case WPAN_SECURITY:
|
||||
if (!ns_capable(net->user_ns, CAP_NET_ADMIN) &&
|
||||
!ns_capable(net->user_ns, CAP_NET_RAW)) {
|
||||
err = -EPERM;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (val) {
|
||||
case WPAN_SECURITY_DEFAULT:
|
||||
ro->secen_override = 0;
|
||||
break;
|
||||
case WPAN_SECURITY_ON:
|
||||
ro->secen_override = 1;
|
||||
ro->secen = 1;
|
||||
break;
|
||||
case WPAN_SECURITY_OFF:
|
||||
ro->secen_override = 1;
|
||||
ro->secen = 0;
|
||||
break;
|
||||
default:
|
||||
err = -EINVAL;
|
||||
break;
|
||||
}
|
||||
break;
|
||||
case WPAN_SECURITY_LEVEL:
|
||||
if (!ns_capable(net->user_ns, CAP_NET_ADMIN) &&
|
||||
!ns_capable(net->user_ns, CAP_NET_RAW)) {
|
||||
err = -EPERM;
|
||||
break;
|
||||
}
|
||||
|
||||
if (val < WPAN_SECURITY_LEVEL_DEFAULT ||
|
||||
val > IEEE802154_SCF_SECLEVEL_ENC_MIC128) {
|
||||
err = -EINVAL;
|
||||
} else if (val == WPAN_SECURITY_LEVEL_DEFAULT) {
|
||||
ro->seclevel_override = 0;
|
||||
} else {
|
||||
ro->seclevel_override = 1;
|
||||
ro->seclevel = val;
|
||||
}
|
||||
break;
|
||||
default:
|
||||
err = -ENOPROTOOPT;
|
||||
break;
|
||||
}
|
||||
|
||||
release_sock(sk);
|
||||
return err;
|
||||
}
|
||||
|
||||
struct proto ieee802154_dgram_prot = {
|
||||
.name = "IEEE-802.15.4-MAC",
|
||||
.owner = THIS_MODULE,
|
||||
.obj_size = sizeof(struct dgram_sock),
|
||||
.init = dgram_init,
|
||||
.close = dgram_close,
|
||||
.bind = dgram_bind,
|
||||
.sendmsg = dgram_sendmsg,
|
||||
.recvmsg = dgram_recvmsg,
|
||||
.hash = dgram_hash,
|
||||
.unhash = dgram_unhash,
|
||||
.connect = dgram_connect,
|
||||
.disconnect = dgram_disconnect,
|
||||
.ioctl = dgram_ioctl,
|
||||
.getsockopt = dgram_getsockopt,
|
||||
.setsockopt = dgram_setsockopt,
|
||||
};
|
||||
|
|
@ -1,270 +0,0 @@
|
|||
/*
|
||||
* Raw IEEE 802.15.4 sockets
|
||||
*
|
||||
* Copyright 2007, 2008 Siemens AG
|
||||
*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License version 2
|
||||
* as published by the Free Software Foundation.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* Written by:
|
||||
* Sergey Lapin <slapin@ossfans.org>
|
||||
* Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>
|
||||
*/
|
||||
|
||||
#include <linux/net.h>
|
||||
#include <linux/module.h>
|
||||
#include <linux/if_arp.h>
|
||||
#include <linux/list.h>
|
||||
#include <linux/slab.h>
|
||||
#include <net/sock.h>
|
||||
#include <net/af_ieee802154.h>
|
||||
#include <net/ieee802154_netdev.h>
|
||||
|
||||
#include "af802154.h"
|
||||
|
||||
static HLIST_HEAD(raw_head);
|
||||
static DEFINE_RWLOCK(raw_lock);
|
||||
|
||||
static void raw_hash(struct sock *sk)
|
||||
{
|
||||
write_lock_bh(&raw_lock);
|
||||
sk_add_node(sk, &raw_head);
|
||||
sock_prot_inuse_add(sock_net(sk), sk->sk_prot, 1);
|
||||
write_unlock_bh(&raw_lock);
|
||||
}
|
||||
|
||||
static void raw_unhash(struct sock *sk)
|
||||
{
|
||||
write_lock_bh(&raw_lock);
|
||||
if (sk_del_node_init(sk))
|
||||
sock_prot_inuse_add(sock_net(sk), sk->sk_prot, -1);
|
||||
write_unlock_bh(&raw_lock);
|
||||
}
|
||||
|
||||
static void raw_close(struct sock *sk, long timeout)
|
||||
{
|
||||
sk_common_release(sk);
|
||||
}
|
||||
|
||||
static int raw_bind(struct sock *sk, struct sockaddr *_uaddr, int len)
|
||||
{
|
||||
struct ieee802154_addr addr;
|
||||
struct sockaddr_ieee802154 *uaddr = (struct sockaddr_ieee802154 *)_uaddr;
|
||||
int err = 0;
|
||||
struct net_device *dev = NULL;
|
||||
|
||||
if (len < sizeof(*uaddr))
|
||||
return -EINVAL;
|
||||
|
||||
uaddr = (struct sockaddr_ieee802154 *)_uaddr;
|
||||
if (uaddr->family != AF_IEEE802154)
|
||||
return -EINVAL;
|
||||
|
||||
lock_sock(sk);
|
||||
|
||||
ieee802154_addr_from_sa(&addr, &uaddr->addr);
|
||||
dev = ieee802154_get_dev(sock_net(sk), &addr);
|
||||
if (!dev) {
|
||||
err = -ENODEV;
|
||||
goto out;
|
||||
}
|
||||
|
||||
if (dev->type != ARPHRD_IEEE802154) {
|
||||
err = -ENODEV;
|
||||
goto out_put;
|
||||
}
|
||||
|
||||
sk->sk_bound_dev_if = dev->ifindex;
|
||||
sk_dst_reset(sk);
|
||||
|
||||
out_put:
|
||||
dev_put(dev);
|
||||
out:
|
||||
release_sock(sk);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
static int raw_connect(struct sock *sk, struct sockaddr *uaddr,
|
||||
int addr_len)
|
||||
{
|
||||
return -ENOTSUPP;
|
||||
}
|
||||
|
||||
static int raw_disconnect(struct sock *sk, int flags)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int raw_sendmsg(struct kiocb *iocb, struct sock *sk,
|
||||
struct msghdr *msg, size_t size)
|
||||
{
|
||||
struct net_device *dev;
|
||||
unsigned int mtu;
|
||||
struct sk_buff *skb;
|
||||
int hlen, tlen;
|
||||
int err;
|
||||
|
||||
if (msg->msg_flags & MSG_OOB) {
|
||||
pr_debug("msg->msg_flags = 0x%x\n", msg->msg_flags);
|
||||
return -EOPNOTSUPP;
|
||||
}
|
||||
|
||||
lock_sock(sk);
|
||||
if (!sk->sk_bound_dev_if)
|
||||
dev = dev_getfirstbyhwtype(sock_net(sk), ARPHRD_IEEE802154);
|
||||
else
|
||||
dev = dev_get_by_index(sock_net(sk), sk->sk_bound_dev_if);
|
||||
release_sock(sk);
|
||||
|
||||
if (!dev) {
|
||||
pr_debug("no dev\n");
|
||||
err = -ENXIO;
|
||||
goto out;
|
||||
}
|
||||
|
||||
mtu = dev->mtu;
|
||||
pr_debug("name = %s, mtu = %u\n", dev->name, mtu);
|
||||
|
||||
if (size > mtu) {
|
||||
pr_debug("size = %Zu, mtu = %u\n", size, mtu);
|
||||
err = -EINVAL;
|
||||
goto out_dev;
|
||||
}
|
||||
|
||||
hlen = LL_RESERVED_SPACE(dev);
|
||||
tlen = dev->needed_tailroom;
|
||||
skb = sock_alloc_send_skb(sk, hlen + tlen + size,
|
||||
msg->msg_flags & MSG_DONTWAIT, &err);
|
||||
if (!skb)
|
||||
goto out_dev;
|
||||
|
||||
skb_reserve(skb, hlen);
|
||||
|
||||
skb_reset_mac_header(skb);
|
||||
skb_reset_network_header(skb);
|
||||
|
||||
err = memcpy_from_msg(skb_put(skb, size), msg, size);
|
||||
if (err < 0)
|
||||
goto out_skb;
|
||||
|
||||
skb->dev = dev;
|
||||
skb->sk = sk;
|
||||
skb->protocol = htons(ETH_P_IEEE802154);
|
||||
|
||||
dev_put(dev);
|
||||
|
||||
err = dev_queue_xmit(skb);
|
||||
if (err > 0)
|
||||
err = net_xmit_errno(err);
|
||||
|
||||
return err ?: size;
|
||||
|
||||
out_skb:
|
||||
kfree_skb(skb);
|
||||
out_dev:
|
||||
dev_put(dev);
|
||||
out:
|
||||
return err;
|
||||
}
|
||||
|
||||
static int raw_recvmsg(struct kiocb *iocb, struct sock *sk, struct msghdr *msg,
|
||||
size_t len, int noblock, int flags, int *addr_len)
|
||||
{
|
||||
size_t copied = 0;
|
||||
int err = -EOPNOTSUPP;
|
||||
struct sk_buff *skb;
|
||||
|
||||
skb = skb_recv_datagram(sk, flags, noblock, &err);
|
||||
if (!skb)
|
||||
goto out;
|
||||
|
||||
copied = skb->len;
|
||||
if (len < copied) {
|
||||
msg->msg_flags |= MSG_TRUNC;
|
||||
copied = len;
|
||||
}
|
||||
|
||||
err = skb_copy_datagram_msg(skb, 0, msg, copied);
|
||||
if (err)
|
||||
goto done;
|
||||
|
||||
sock_recv_ts_and_drops(msg, sk, skb);
|
||||
|
||||
if (flags & MSG_TRUNC)
|
||||
copied = skb->len;
|
||||
done:
|
||||
skb_free_datagram(sk, skb);
|
||||
out:
|
||||
if (err)
|
||||
return err;
|
||||
return copied;
|
||||
}
|
||||
|
||||
static int raw_rcv_skb(struct sock *sk, struct sk_buff *skb)
|
||||
{
|
||||
skb = skb_share_check(skb, GFP_ATOMIC);
|
||||
if (!skb)
|
||||
return NET_RX_DROP;
|
||||
|
||||
if (sock_queue_rcv_skb(sk, skb) < 0) {
|
||||
kfree_skb(skb);
|
||||
return NET_RX_DROP;
|
||||
}
|
||||
|
||||
return NET_RX_SUCCESS;
|
||||
}
|
||||
|
||||
void ieee802154_raw_deliver(struct net_device *dev, struct sk_buff *skb)
|
||||
{
|
||||
struct sock *sk;
|
||||
|
||||
read_lock(&raw_lock);
|
||||
sk_for_each(sk, &raw_head) {
|
||||
bh_lock_sock(sk);
|
||||
if (!sk->sk_bound_dev_if ||
|
||||
sk->sk_bound_dev_if == dev->ifindex) {
|
||||
struct sk_buff *clone;
|
||||
|
||||
clone = skb_clone(skb, GFP_ATOMIC);
|
||||
if (clone)
|
||||
raw_rcv_skb(sk, clone);
|
||||
}
|
||||
bh_unlock_sock(sk);
|
||||
}
|
||||
read_unlock(&raw_lock);
|
||||
}
|
||||
|
||||
static int raw_getsockopt(struct sock *sk, int level, int optname,
|
||||
char __user *optval, int __user *optlen)
|
||||
{
|
||||
return -EOPNOTSUPP;
|
||||
}
|
||||
|
||||
static int raw_setsockopt(struct sock *sk, int level, int optname,
|
||||
char __user *optval, unsigned int optlen)
|
||||
{
|
||||
return -EOPNOTSUPP;
|
||||
}
|
||||
|
||||
struct proto ieee802154_raw_prot = {
|
||||
.name = "IEEE-802.15.4-RAW",
|
||||
.owner = THIS_MODULE,
|
||||
.obj_size = sizeof(struct sock),
|
||||
.close = raw_close,
|
||||
.bind = raw_bind,
|
||||
.sendmsg = raw_sendmsg,
|
||||
.recvmsg = raw_recvmsg,
|
||||
.hash = raw_hash,
|
||||
.unhash = raw_unhash,
|
||||
.connect = raw_connect,
|
||||
.disconnect = raw_disconnect,
|
||||
.getsockopt = raw_getsockopt,
|
||||
.setsockopt = raw_setsockopt,
|
||||
};
|
|
@ -1,41 +0,0 @@
|
|||
#ifndef __IEEE802154_6LOWPAN_REASSEMBLY_H__
|
||||
#define __IEEE802154_6LOWPAN_REASSEMBLY_H__
|
||||
|
||||
#include <net/inet_frag.h>
|
||||
|
||||
struct lowpan_create_arg {
|
||||
u16 tag;
|
||||
u16 d_size;
|
||||
const struct ieee802154_addr *src;
|
||||
const struct ieee802154_addr *dst;
|
||||
};
|
||||
|
||||
/* Equivalent of ipv4 struct ip
|
||||
*/
|
||||
struct lowpan_frag_queue {
|
||||
struct inet_frag_queue q;
|
||||
|
||||
u16 tag;
|
||||
u16 d_size;
|
||||
struct ieee802154_addr saddr;
|
||||
struct ieee802154_addr daddr;
|
||||
};
|
||||
|
||||
static inline u32 ieee802154_addr_hash(const struct ieee802154_addr *a)
|
||||
{
|
||||
switch (a->mode) {
|
||||
case IEEE802154_ADDR_LONG:
|
||||
return (((__force u64)a->extended_addr) >> 32) ^
|
||||
(((__force u64)a->extended_addr) & 0xffffffff);
|
||||
case IEEE802154_ADDR_SHORT:
|
||||
return (__force u32)(a->short_addr);
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
int lowpan_frag_rcv(struct sk_buff *skb, const u8 frag_type);
|
||||
void lowpan_net_frag_exit(void);
|
||||
int lowpan_net_frag_init(void);
|
||||
|
||||
#endif /* __IEEE802154_6LOWPAN_REASSEMBLY_H__ */
|
1125
net/ieee802154/socket.c
Normal file
1125
net/ieee802154/socket.c
Normal file
File diff suppressed because it is too large
Load Diff
|
@ -51,10 +51,7 @@ ieee802154_add_iface(struct wpan_phy *phy, const char *name,
|
|||
struct net_device *err;
|
||||
|
||||
err = ieee802154_if_add(local, name, type, extended_addr);
|
||||
if (IS_ERR(err))
|
||||
return PTR_ERR(err);
|
||||
|
||||
return 0;
|
||||
return PTR_ERR_OR_ZERO(err);
|
||||
}
|
||||
|
||||
static int
|
||||
|
|
Loading…
Reference in New Issue
Block a user