1 /* $NetBSD: pcap-bt-monitor-linux.c,v 1.3 2015/03/31 21:39:42 christos Exp $ */ 2 3 /* 4 * Copyright (c) 2014 Michal Labedzki for Tieto Corporation 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 11 * 1. Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * 2. Redistributions in binary form must reproduce the above copyright 14 * notice, this list of conditions and the following disclaimer in the 15 * documentation and/or other materials provided with the distribution. 16 * 3. The name of the author may not be used to endorse or promote 17 * products derived from this software without specific prior written 18 * permission. 19 * 20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR 23 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 24 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 25 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 26 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, 27 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY 28 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 30 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 31 * 32 */ 33 34 #include <sys/cdefs.h> 35 __RCSID("$NetBSD: pcap-bt-monitor-linux.c,v 1.3 2015/03/31 21:39:42 christos Exp $"); 36 37 #ifdef HAVE_CONFIG_H 38 #include "config.h" 39 #endif 40 41 #include <errno.h> 42 #include <stdlib.h> 43 #include <string.h> 44 45 #include <bluetooth/bluetooth.h> 46 #include <bluetooth/hci.h> 47 #include <bluetooth/mgmt.h> 48 49 #include "pcap/bluetooth.h" 50 #include "pcap-int.h" 51 52 #include "pcap-bt-monitor-linux.h" 53 54 #define BT_CONTROL_SIZE 32 55 #define INTERFACE_NAME "bluetooth-monitor" 56 57 int 58 bt_monitor_findalldevs(pcap_if_t **alldevsp, char *err_str) 59 { 60 int ret = 0; 61 62 if (pcap_add_if(alldevsp, INTERFACE_NAME, 0, 63 "Bluetooth Linux Monitor", err_str) < 0) 64 { 65 ret = -1; 66 } 67 68 return ret; 69 } 70 71 static int 72 bt_monitor_read(pcap_t *handle, int max_packets _U_, pcap_handler callback, u_char *user) 73 { 74 struct cmsghdr *cmsg; 75 struct msghdr msg; 76 struct iovec iv[2]; 77 ssize_t ret; 78 struct pcap_pkthdr pkth; 79 pcap_bluetooth_linux_monitor_header *bthdr; 80 struct mgmt_hdr hdr; 81 82 bthdr = (pcap_bluetooth_linux_monitor_header*) &handle->buffer[handle->offset]; 83 84 iv[0].iov_base = &hdr; 85 iv[0].iov_len = MGMT_HDR_SIZE; 86 iv[1].iov_base = &handle->buffer[handle->offset + sizeof(pcap_bluetooth_linux_monitor_header)]; 87 iv[1].iov_len = handle->snapshot; 88 89 memset(&pkth.ts, 0, sizeof(pkth.ts)); 90 memset(&msg, 0, sizeof(msg)); 91 msg.msg_iov = iv; 92 msg.msg_iovlen = 2; 93 msg.msg_control = handle->buffer; 94 msg.msg_controllen = handle->offset; 95 96 do { 97 ret = recvmsg(handle->fd, &msg, 0); 98 if (handle->break_loop) 99 { 100 handle->break_loop = 0; 101 return -2; 102 } 103 } while ((ret == -1) && (errno == EINTR)); 104 105 if (ret < 0) { 106 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, 107 "Can't receive packet: %s", strerror(errno)); 108 return -1; 109 } 110 111 pkth.caplen = ret - MGMT_HDR_SIZE + sizeof(pcap_bluetooth_linux_monitor_header); 112 pkth.len = pkth.caplen; 113 114 for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL; cmsg = CMSG_NXTHDR(&msg, cmsg)) { 115 if (cmsg->cmsg_level != SOL_SOCKET) continue; 116 117 if (cmsg->cmsg_type == SCM_TIMESTAMP) { 118 memcpy(&pkth.ts, CMSG_DATA(cmsg), sizeof(pkth.ts)); 119 } 120 } 121 122 bthdr->adapter_id = htons(hdr.index); 123 bthdr->opcode = htons(hdr.opcode); 124 125 if (handle->fcode.bf_insns == NULL || 126 bpf_filter(handle->fcode.bf_insns, &handle->buffer[handle->offset], 127 pkth.len, pkth.caplen)) { 128 callback(user, &pkth, &handle->buffer[handle->offset]); 129 return 1; 130 } 131 return 0; /* didn't pass filter */ 132 } 133 134 static int 135 bt_monitor_inject(pcap_t *handle, const void *buf _U_, size_t size _U_) 136 { 137 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "inject not supported yet"); 138 return -1; 139 } 140 141 static int 142 bt_monitor_setdirection(pcap_t *p, pcap_direction_t d) 143 { 144 p->direction = d; 145 return 0; 146 } 147 148 static int 149 bt_monitor_stats(pcap_t *handle _U_, struct pcap_stat *stats) 150 { 151 stats->ps_recv = 0; 152 stats->ps_drop = 0; 153 stats->ps_ifdrop = 0; 154 155 return 0; 156 } 157 158 static int 159 bt_monitor_activate(pcap_t* handle) 160 { 161 struct sockaddr_hci addr; 162 int err = PCAP_ERROR; 163 int opt; 164 165 if (handle->opt.rfmon) { 166 /* monitor mode doesn't apply here */ 167 return PCAP_ERROR_RFMON_NOTSUP; 168 } 169 170 handle->bufsize = handle->snapshot + BT_CONTROL_SIZE + sizeof(pcap_bluetooth_linux_monitor_header); 171 handle->offset = BT_CONTROL_SIZE; 172 handle->linktype = DLT_BLUETOOTH_LINUX_MONITOR; 173 174 handle->read_op = bt_monitor_read; 175 handle->inject_op = bt_monitor_inject; 176 handle->setfilter_op = install_bpf_program; /* no kernel filtering */ 177 handle->setdirection_op = bt_monitor_setdirection; 178 handle->set_datalink_op = NULL; /* can't change data link type */ 179 handle->getnonblock_op = pcap_getnonblock_fd; 180 handle->setnonblock_op = pcap_setnonblock_fd; 181 handle->stats_op = bt_monitor_stats; 182 183 handle->fd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI); 184 if (handle->fd < 0) { 185 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, 186 "Can't create raw socket: %s", strerror(errno)); 187 return PCAP_ERROR; 188 } 189 190 handle->buffer = malloc(handle->bufsize); 191 if (!handle->buffer) { 192 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, "Can't allocate dump buffer: %s", 193 pcap_strerror(errno)); 194 goto close_fail; 195 } 196 197 /* Bind socket to the HCI device */ 198 addr.hci_family = AF_BLUETOOTH; 199 addr.hci_dev = HCI_DEV_NONE; 200 addr.hci_channel = HCI_CHANNEL_MONITOR; 201 202 if (bind(handle->fd, (struct sockaddr *) &addr, sizeof(addr)) < 0) { 203 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, 204 "Can't attach to interface: %s", strerror(errno)); 205 goto close_fail; 206 } 207 208 opt = 1; 209 if (setsockopt(handle->fd, SOL_SOCKET, SO_TIMESTAMP, &opt, sizeof(opt)) < 0) { 210 snprintf(handle->errbuf, PCAP_ERRBUF_SIZE, 211 "Can't enable time stamp: %s", strerror(errno)); 212 goto close_fail; 213 } 214 215 handle->selectable_fd = handle->fd; 216 217 return 0; 218 219 close_fail: 220 pcap_cleanup_live_common(handle); 221 return err; 222 } 223 224 pcap_t * 225 bt_monitor_create(const char *device, char *ebuf, int *is_ours) 226 { 227 pcap_t *p; 228 const char *cp; 229 230 cp = strrchr(device, '/'); 231 if (cp == NULL) 232 cp = device; 233 234 if (strcmp(cp, INTERFACE_NAME) != 0) { 235 *is_ours = 0; 236 return NULL; 237 } 238 239 *is_ours = 1; 240 p = pcap_create_common(device, ebuf, 0); 241 if (p == NULL) 242 return NULL; 243 244 p->activate_op = bt_monitor_activate; 245 246 return p; 247 } 248