diff options
author | Adrian Bunk <adrian.bunk@movial.com> | 2011-06-03 09:17:04 +0000 |
---|---|---|
committer | Adrian Bunk <adrian.bunk@movial.com> | 2011-06-03 09:17:04 +0000 |
commit | 799757ccf1d03c33c75bc597cd5ef77741dcb6a7 (patch) | |
tree | a8c3be85c730de28b012586591b76301033d3d21 /compat/bnep.c |
Imported upstream 4.91upstream-4.91upstreampackaging
Diffstat (limited to 'compat/bnep.c')
-rw-r--r-- | compat/bnep.c | 339 |
1 files changed, 339 insertions, 0 deletions
diff --git a/compat/bnep.c b/compat/bnep.c new file mode 100644 index 0000000..281350b --- /dev/null +++ b/compat/bnep.c @@ -0,0 +1,339 @@ +/* + * + * BlueZ - Bluetooth protocol stack for Linux + * + * Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com> + * Copyright (C) 2002-2010 Marcel Holtmann <marcel@holtmann.org> + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#ifdef HAVE_CONFIG_H +#include <config.h> +#endif + +#include <stdio.h> +#include <errno.h> +#include <unistd.h> +#include <stdlib.h> +#include <sys/socket.h> +#include <sys/ioctl.h> + +#include <bluetooth/bluetooth.h> +#include <bluetooth/bnep.h> + +#include <netinet/in.h> + +#include "pand.h" + +static int ctl; + +/* Compatibility with old ioctls */ +#define OLD_BNEPCONADD 1 +#define OLD_BNEPCONDEL 2 +#define OLD_BNEPGETCONLIST 3 +#define OLD_BNEPGETCONINFO 4 + +static unsigned long bnepconnadd; +static unsigned long bnepconndel; +static unsigned long bnepgetconnlist; +static unsigned long bnepgetconninfo; + +static struct { + char *str; + uint16_t uuid; +} __svc[] = { + { "PANU", BNEP_SVC_PANU }, + { "NAP", BNEP_SVC_NAP }, + { "GN", BNEP_SVC_GN }, + { NULL } +}; + +int bnep_str2svc(char *svc, uint16_t *uuid) +{ + int i; + for (i = 0; __svc[i].str; i++) + if (!strcasecmp(svc, __svc[i].str)) { + *uuid = __svc[i].uuid; + return 0; + } + return -1; +} + +char *bnep_svc2str(uint16_t uuid) +{ + int i; + for (i = 0; __svc[i].str; i++) + if (__svc[i].uuid == uuid) + return __svc[i].str; + return NULL; +} + +int bnep_init(void) +{ + ctl = socket(PF_BLUETOOTH, SOCK_RAW, BTPROTO_BNEP); + if (ctl < 0) { + perror("Failed to open control socket"); + return 1; + } + + /* Temporary ioctl compatibility hack */ + { + struct bnep_connlist_req req; + struct bnep_conninfo ci[1]; + + req.cnum = 1; + req.ci = ci; + + if (!ioctl(ctl, BNEPGETCONNLIST, &req)) { + /* New ioctls */ + bnepconnadd = BNEPCONNADD; + bnepconndel = BNEPCONNDEL; + bnepgetconnlist = BNEPGETCONNLIST; + bnepgetconninfo = BNEPGETCONNINFO; + } else { + /* Old ioctls */ + bnepconnadd = OLD_BNEPCONADD; + bnepconndel = OLD_BNEPCONDEL; + bnepgetconnlist = OLD_BNEPGETCONLIST; + bnepgetconninfo = OLD_BNEPGETCONINFO; + } + } + + return 0; +} + +int bnep_cleanup(void) +{ + close(ctl); + return 0; +} + +int bnep_show_connections(void) +{ + struct bnep_connlist_req req; + struct bnep_conninfo ci[48]; + unsigned int i; + + req.cnum = 48; + req.ci = ci; + if (ioctl(ctl, bnepgetconnlist, &req)) { + perror("Failed to get connection list"); + return -1; + } + + for (i = 0; i < req.cnum; i++) { + char addr[18]; + ba2str((bdaddr_t *) ci[i].dst, addr); + printf("%s %s %s\n", ci[i].device, + addr, bnep_svc2str(ci[i].role)); + } + return 0; +} + +int bnep_kill_connection(uint8_t *dst) +{ + struct bnep_conndel_req req; + + memcpy(req.dst, dst, ETH_ALEN); + req.flags = 0; + if (ioctl(ctl, bnepconndel, &req)) { + perror("Failed to kill connection"); + return -1; + } + return 0; +} + +int bnep_kill_all_connections(void) +{ + struct bnep_connlist_req req; + struct bnep_conninfo ci[48]; + unsigned int i; + + req.cnum = 48; + req.ci = ci; + if (ioctl(ctl, bnepgetconnlist, &req)) { + perror("Failed to get connection list"); + return -1; + } + + for (i = 0; i < req.cnum; i++) { + struct bnep_conndel_req req; + memcpy(req.dst, ci[i].dst, ETH_ALEN); + req.flags = 0; + ioctl(ctl, bnepconndel, &req); + } + return 0; +} + +static int bnep_connadd(int sk, uint16_t role, char *dev) +{ + struct bnep_connadd_req req; + + strncpy(req.device, dev, 16); + req.device[15] = '\0'; + req.sock = sk; + req.role = role; + if (ioctl(ctl, bnepconnadd, &req)) + return -1; + strncpy(dev, req.device, 16); + return 0; +} + +struct __service_16 { + uint16_t dst; + uint16_t src; +} __attribute__ ((packed)); + +struct __service_32 { + uint16_t unused1; + uint16_t dst; + uint16_t unused2; + uint16_t src; +} __attribute__ ((packed)); + +struct __service_128 { + uint16_t unused1; + uint16_t dst; + uint16_t unused2[8]; + uint16_t src; + uint16_t unused3[7]; +} __attribute__ ((packed)); + +int bnep_accept_connection(int sk, uint16_t role, char *dev) +{ + struct bnep_setup_conn_req *req; + struct bnep_control_rsp *rsp; + unsigned char pkt[BNEP_MTU]; + ssize_t r; + + r = recv(sk, pkt, BNEP_MTU, 0); + if (r <= 0) + return -1; + + errno = EPROTO; + + if ((size_t) r < sizeof(*req)) + return -1; + + req = (void *) pkt; + + /* Highest known Control command ID + * is BNEP_FILTER_MULT_ADDR_RSP = 0x06 */ + if (req->type == BNEP_CONTROL && + req->ctrl > BNEP_FILTER_MULT_ADDR_RSP) { + uint8_t pkt[3]; + + pkt[0] = BNEP_CONTROL; + pkt[1] = BNEP_CMD_NOT_UNDERSTOOD; + pkt[2] = req->ctrl; + + send(sk, pkt, sizeof(pkt), 0); + + return -1; + } + + if (req->type != BNEP_CONTROL || req->ctrl != BNEP_SETUP_CONN_REQ) + return -1; + + /* FIXME: Check role UUIDs */ + + rsp = (void *) pkt; + rsp->type = BNEP_CONTROL; + rsp->ctrl = BNEP_SETUP_CONN_RSP; + rsp->resp = htons(BNEP_SUCCESS); + if (send(sk, rsp, sizeof(*rsp), 0) < 0) + return -1; + + return bnep_connadd(sk, role, dev); +} + +/* Create BNEP connection + * sk - Connect L2CAP socket + * role - Local role + * service - Remote service + * dev - Network device (contains actual dev name on return) + */ +int bnep_create_connection(int sk, uint16_t role, uint16_t svc, char *dev) +{ + struct bnep_setup_conn_req *req; + struct bnep_control_rsp *rsp; + struct __service_16 *s; + struct timeval timeo; + unsigned char pkt[BNEP_MTU]; + ssize_t r; + + /* Send request */ + req = (void *) pkt; + req->type = BNEP_CONTROL; + req->ctrl = BNEP_SETUP_CONN_REQ; + req->uuid_size = 2; /* 16bit UUID */ + + s = (void *) req->service; + s->dst = htons(svc); + s->src = htons(role); + + memset(&timeo, 0, sizeof(timeo)); + timeo.tv_sec = 30; + + setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &timeo, sizeof(timeo)); + + if (send(sk, pkt, sizeof(*req) + sizeof(*s), 0) < 0) + return -1; + +receive: + /* Get response */ + r = recv(sk, pkt, BNEP_MTU, 0); + if (r <= 0) + return -1; + + memset(&timeo, 0, sizeof(timeo)); + timeo.tv_sec = 0; + + setsockopt(sk, SOL_SOCKET, SO_RCVTIMEO, &timeo, sizeof(timeo)); + + errno = EPROTO; + + if ((size_t) r < sizeof(*rsp)) + return -1; + + rsp = (void *) pkt; + if (rsp->type != BNEP_CONTROL) + return -1; + + if (rsp->ctrl != BNEP_SETUP_CONN_RSP) + goto receive; + + r = ntohs(rsp->resp); + + switch (r) { + case BNEP_SUCCESS: + break; + + case BNEP_CONN_INVALID_DST: + case BNEP_CONN_INVALID_SRC: + case BNEP_CONN_INVALID_SVC: + errno = EPROTO; + return -1; + + case BNEP_CONN_NOT_ALLOWED: + errno = EACCES; + return -1; + } + + return bnep_connadd(sk, role, dev); +} |