static int getbdaddrbyname(char *str, bdaddr_t *ba)
{
int i, n, len;
len = strlen(str);
/* Check address format */
for (i = 0, n = 0; i < len; i++)
if (str[i] == ':')
n++;
if (n == 5) {
/* BD address */
str2ba(str, ba);
return 0;
}
if (n == 0) {
/* loopback port */
in_addr_t addr = INADDR_LOOPBACK;
uint16_t be16 = htons(atoi(str));
bdaddr_t b;
memcpy(&b, &addr, 4);
memcpy(&b.b[4], &be16, sizeof(be16));
baswap(ba, &b);
return 0;
}
fprintf(stderr, "Invalid address format\n");
return -1;
}
static ssize_t show_conn_address(struct device *dev, struct device_attribute *attr, char *buf)
{
struct hci_conn *conn = dev_get_drvdata(dev);
bdaddr_t bdaddr;
baswap(&bdaddr, &conn->dst);
return sprintf(buf, "%s\n", batostr(&bdaddr));
}
static ssize_t show_address(struct device *dev, struct device_attribute *attr, char *buf)
{
struct hci_dev *hdev = dev_get_drvdata(dev);
bdaddr_t bdaddr;
baswap(&bdaddr, &hdev->bdaddr);
return sprintf(buf, "%s\n", batostr(&bdaddr));
}
int ba2str(const bdaddr_t *ba, char *str)
{
uint8_t b[6];
baswap((bdaddr_t *) b, ba);
return sprintf(str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
b[0], b[1], b[2], b[3], b[4], b[5]);
}
int ba2oui(const bdaddr_t *ba, char *str)
{
uint8_t b[6];
baswap((bdaddr_t *) b, ba);
return sprintf(str, "%2.2X-%2.2X-%2.2X", b[0], b[1], b[2]);
}
static int getbdaddrbyname(char *str, bdaddr_t *ba)
{
int i, n, len;
len = strlen(str);
/* Check address format */
for (i = 0, n = 0; i < len; i++)
if (str[i] == ':')
n++;
if (n == 5) {
/* BD address */
baswap(ba, strtoba(str));
return 0;
}
if (n == 1) {
/* IP address + port */
struct hostent *hent;
bdaddr_t b;
char *ptr;
ptr = strchr(str, ':');
*ptr++ = 0;
if (!(hent = gethostbyname(str))) {
fprintf(stderr, "Can't resolve %s\n", str);
return -2;
}
memcpy(&b, hent->h_addr, 4);
*(uint16_t *) (&b.b[4]) = htons(atoi(ptr));
baswap(ba, &b);
return 0;
}
fprintf(stderr, "Invalid address format\n");
return -1;
}
static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
u8 _rat, bdaddr_t *ra, u8 res[16])
{
u8 p1[16], p2[16];
int err;
memset(p1, 0, 16);
/* p1 = pres || preq || _rat || _iat */
swap56(pres, p1);
swap56(preq, p1 + 7);
p1[14] = _rat;
p1[15] = _iat;
memset(p2, 0, 16);
/* p2 = padding || ia || ra */
baswap((bdaddr_t *) (p2 + 4), ia);
baswap((bdaddr_t *) (p2 + 10), ra);
/* res = r XOR p1 */
u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
/* res = e(k, res) */
err = smp_e(tfm, k, res);
if (err) {
BT_ERR("Encrypt data error");
return err;
}
/* res = res XOR p2 */
u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
/* res = e(k, res) */
err = smp_e(tfm, k, res);
if (err)
BT_ERR("Encrypt data error");
return err;
}
static int smp_c1(struct crypto_blkcipher *tfm, u8 k[16], u8 r[16],
u8 preq[7], u8 pres[7], u8 _iat, bdaddr_t *ia,
u8 _rat, bdaddr_t *ra, u8 res[16])
{
u8 p1[16], p2[16];
int err;
memset(p1, 0, 16);
swap56(pres, p1);
swap56(preq, p1 + 7);
p1[14] = _rat;
p1[15] = _iat;
memset(p2, 0, 16);
baswap((bdaddr_t *) (p2 + 4), ia);
baswap((bdaddr_t *) (p2 + 10), ra);
u128_xor((u128 *) res, (u128 *) r, (u128 *) p1);
err = smp_e(tfm, k, res);
if (err) {
BT_ERR("Encrypt data error");
return err;
}
u128_xor((u128 *) res, (u128 *) res, (u128 *) p2);
err = smp_e(tfm, k, res);
if (err)
BT_ERR("Encrypt data error");
return err;
}
bdaddr_t *strtoba(const char *str)
{
bdaddr_t b;
bdaddr_t *ba = bt_malloc(sizeof(*ba));
if (ba) {
str2ba(str, &b);
baswap(ba, &b);
}
return ba;
}
static int scan_enable(uint8_t *data)
{
struct sockaddr_in sa;
GIOChannel *sk_io;
bdaddr_t ba;
int sk, opt;
if (!(*data & SCAN_PAGE)) {
if (vdev.scan) {
g_io_channel_shutdown(vdev.scan, TRUE, NULL);
vdev.scan = NULL;
}
return 0;
}
if (vdev.scan)
return 0;
if ((sk = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
return 1;
}
opt = 1;
setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
baswap(&ba, &vdev.bdaddr);
sa.sin_family = AF_INET;
memcpy(&sa.sin_addr.s_addr, &ba, sizeof(sa.sin_addr.s_addr));
sa.sin_port = *(uint16_t *) &ba.b[4];
if (bind(sk, (struct sockaddr *) &sa, sizeof(sa))) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto failed;
}
if (listen(sk, 10)) {
syslog(LOG_ERR, "Can't listen on socket: %s (%d)",
strerror(errno), errno);
goto failed;
}
sk_io = g_io_channel_unix_new(sk);
g_io_add_watch(sk_io, G_IO_IN | G_IO_NVAL, io_conn_ind, NULL);
vdev.scan = sk_io;
return 0;
failed:
close(sk);
return 1;
}
int main (int argc,char **argv)
{
struct sockaddr_rc loc_addr ={0},rem_addr={0};
char buf[1024] ={0},*addr;
int s,client, bytes_read,result;
int opt = sizeof(rem_addr);
printf("Creating socket...\n");
s=socket(AF_BLUETOOTH,SOCK_STREAM,BTPROTO_RFCOMM);
if(s<0)
{
perror("create socket error");
exit(1);
}
else
{
printf("success!\n");
}
loc_addr.rc_family=AF_BLUETOOTH;
loc_addr.rc_bdaddr=*BDADDR_ANY;
loc_addr.rc_channel=(uint8_t)1;
printf("Binding socket...\n");
result=bind(s,(struct sockaddr *)&loc_addr, sizeof(loc_addr));
if(result<0)
{
perror("bind socket error:");
exit(1);
}
else
{
printf("success!\n");
}
void baswap(bdaddr_t *dst, const bdaddr_t *src)
{
register unsigned char *d = (unsigned char *) dst;
register const unsigned char *s = (const unsigned char *) src;
register int i;
for (i = 0; i < 6; i++)
d[i] = s[5-i];
}
int ba2str(const bdaddr_t *ba, char *str)
{
uint8_t b[6];
baswap((bdaddr_t *) b, ba);
return sprintf(str, "%2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
b[0], b[1], b[2], b[3], b[4], b[5]);
}
static int bnep_conndel(const bdaddr_t *dst)
{
struct bnep_conndel_req req;
memset(&req, 0, sizeof(req));
baswap((bdaddr_t *)&req.dst, dst);
req.flags = 0;
if (ioctl(ctl, BNEPCONNDEL, &req)) {
int err = -errno;
error("Failed to kill connection: %s (%d)",
strerror(-err), -err);
return err;
}
return 0;
}
int str2ba(const char *str, bdaddr_t *ba)
{
uint8_t b[6];
const char *ptr = str;
int i;
for (i = 0; i < 6; i++) {
b[i] = (uint8_t) strtol(ptr, NULL, 16);
if (i != 5 && !(ptr = strchr(ptr, ':')))
ptr = ":00:00:00:00:00";
ptr++;
}
baswap(ba, (bdaddr_t *) b);
return 0;
}
static int set_master_bdaddr(int fd, const bdaddr_t *bdaddr)
{
uint8_t buf[8];
int ret;
buf[0] = 0xf5;
buf[1] = 0x01;
baswap((bdaddr_t *) (buf + 2), bdaddr);
ret = ioctl(fd, HIDIOCSFEATURE(sizeof(buf)), buf);
if (ret < 0)
error("sixaxis: failed to write master address (%s)",
strerror(errno));
return ret;
}
int str2ba(const char *str, bdaddr_t *ba)
{
bdaddr_t b;
int i;
if (bachk(str) < 0) {
memset(ba, 0, sizeof(*ba));
return -1;
}
for (i = 0; i < 6; i++, str += 3)
b.b[i] = strtol(str, NULL, 16);
baswap(ba, &b);
return 0;
}
int synchroServeur(char* addr){
struct sockaddr_l2 addrCarte = {0};
char* buff = "Synchro";
int sock;
baswap(&addrCarte.l2_bdaddr,strtoba(addr));
addrCarte.l2_family = AF_BLUETOOTH;
addrCarte.l2_psm = htobs(PORT);
if ((sock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0) {
perror("Synchro socket create");
return;
}
if(connect(sock, (struct sockaddr*)&addrCarte, sizeof(addrCarte))<0){
perror("Synchro connect");
return;
}
write (sock, buff, sizeof(buff));
close(sock);
}
static int get_device_bdaddr(int fd, bdaddr_t *bdaddr)
{
uint8_t buf[18];
int ret;
memset(buf, 0, sizeof(buf));
buf[0] = 0xf2;
ret = ioctl(fd, HIDIOCGFEATURE(sizeof(buf)), buf);
if (ret < 0) {
error("sixaxis: failed to read device address (%s)",
strerror(errno));
return ret;
}
baswap(bdaddr, (bdaddr_t *) (buf + 4));
return 0;
}
static ssize_t show_inquiry_cache(struct device *dev, struct device_attribute *attr, char *buf)
{
struct hci_dev *hdev = dev_get_drvdata(dev);
struct inquiry_cache *cache = &hdev->inq_cache;
struct inquiry_entry *e;
int n = 0;
hci_dev_lock_bh(hdev);
for (e = cache->list; e; e = e->next) {
struct inquiry_data *data = &e->data;
bdaddr_t bdaddr;
baswap(&bdaddr, &data->bdaddr);
n += sprintf(buf + n, "%s %d %d %d 0x%.2x%.2x%.2x 0x%.4x %d %u\n",
batostr(&bdaddr),
data->pscan_rep_mode, data->pscan_period_mode, data->pscan_mode,
data->dev_class[2], data->dev_class[1], data->dev_class[0],
__le16_to_cpu(data->clock_offset), data->rssi, e->timestamp);
}
hci_dev_unlock_bh(hdev);
return n;
}
int scanBt(){
inquiry_info* info = NULL;
int max_rsp = 255;
int flags = IREQ_CACHE_FLUSH;
int num_rsp = 0;
bdaddr_t addr;
int i, sock;
int DEVIDHCI=0;
info = malloc(max_rsp * sizeof(inquiry_info));
memset(info, 0, max_rsp * sizeof(inquiry_info));
num_rsp = hci_inquiry(DEVIDHCI, 8, num_rsp, NULL, &info, flags);
sock = hci_open_dev(DEVIDHCI);
if (num_rsp < 0) {
perror("hci_inquiry");
}
for (i = 0; i < num_rsp; i++) {
baswap(&addr, &(info + i)->bdaddr);
printf("Addr : %s\n", batostr(&addr));
}
close(sock);
}
bool
K2sendPlayer::blueOpen ()
{
struct sockaddr_l2 rem_addr, loc_addr;
struct l2cap_options opts;
//bdaddr_t bdaddr;
int opt;
char buffer[128];
QString msg;
kdDebug (200010) << "K2sendPlayer::blueOpen called" << endl;
is_connected = FALSE;
if (baddr.isEmpty ()) {
msg = QString ("No address");
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 3" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 4" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
msg = QString ("Try to connect " + baddr);
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 30000);
//kdDebug(200010) << "post 5" << endl;
QApplication::postEvent (m_parent, se);
/* create a bluetooth socket */
if ((blue_sock = socket (PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0) {
msg = QString ("Can't create socket");
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 6" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 7" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
kdDebug (200010) << "K2sendPlayer::blueOpen got socket" << endl;
/* setup control structures */
memset (&loc_addr, 0, sizeof (loc_addr));
loc_addr.l2_family = AF_BLUETOOTH;
loc_addr.l2_psm = htobs (MP3_PSM);
if (bind (blue_sock, (struct sockaddr *) &loc_addr, sizeof (loc_addr)) < 0) {
msg = QString ("Can't bind socket");
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
QApplication::postEvent (m_parent, se);
//kdDebug(200010) << "post 8" << endl;
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 9" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
kdDebug (200010) << "K2sendPlayer::blueOpen bind done" << endl;
memset (&rem_addr, 0, sizeof (rem_addr));
rem_addr.l2_family = AF_BLUETOOTH;
baswap (&rem_addr.l2_bdaddr, strtoba (baddr.latin1 ()));
rem_addr.l2_psm = htobs (MP3_PSM);
if (connect (blue_sock, (struct sockaddr *) &rem_addr, sizeof (rem_addr)) < 0) {
msg = QString ("Can't connect.");
close (blue_sock);
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 10" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 11" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
kdDebug (200010) << "K2sendPlayer::blueOpen connect ok" << endl;
opt = sizeof (opts);
if (getsockopt (blue_sock, SOL_L2CAP, L2CAP_OPTIONS, &opts, (socklen_t *) & opt) < 0) {
msg = QString ("Can't get L2CAP options. ");
close (blue_sock);
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 12" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 13" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
/* use omtu for output buffer size */
buffer_size = opts.omtu;
/* ask for firmware version */
buffer[0] = K2sendPlayerCommand::Fwrev;
if (write (blue_sock, buffer, 1) != 1) {
msg = QString ("Failed to write firmware request");
close (blue_sock);
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 14" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 15" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
kdDebug (200010) << "K2sendPlayer::blueOpen req fw done" << endl;
if (read (blue_sock, buffer, sizeof (buffer)) < 0) {
msg = QString ("Failed to read firmware reply");
close (blue_sock);
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 16" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 17" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
kdDebug (200010) << "K2sendPlayer::blueOpen got response" << endl;
if (buffer[0] != K2sendPlayerCommand::Fwrev) {
msg = QString ("Unexpected firware reply");
K2sendStatusEvent *se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 17" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventError, msg);
//kdDebug(200010) << "post 18" << endl;
QApplication::postEvent (m_parent, se);
return FALSE;
}
is_connected = TRUE;
msg = QString ("Connected");
se = new K2sendStatusEvent (K2sendStatusEvent::EventMessage, msg, 2000);
//kdDebug(200010) << "post 19" << endl;
QApplication::postEvent (m_parent, se);
se = new K2sendStatusEvent (K2sendStatusEvent::EventAddr, baddr);
//kdDebug(200010) << "post 20" << endl;
QApplication::postEvent (m_parent, se);
return TRUE;
}
static uint8_t scan_enable(uint8_t *data)
{
#if 0
struct epoll_event scan_event;
struct sockaddr_in sa;
bdaddr_t ba;
int sk, opt;
if (!(*data & SCAN_PAGE)) {
if (vdev.scan_fd >= 0) {
close(vdev.scan_fd);
vdev.scan_fd = -1;
}
return 0;
}
if (vdev.scan_fd >= 0)
return 0;
if ((sk = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
return 1;
}
opt = 1;
setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
baswap(&ba, &vdev.bdaddr);
sa.sin_family = AF_INET;
memcpy(&sa.sin_addr.s_addr, &ba, sizeof(sa.sin_addr.s_addr));
memcpy(&sa.sin_port, &ba.b[4], sizeof(sa.sin_port));
if (bind(sk, (struct sockaddr *) &sa, sizeof(sa))) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto failed;
}
if (listen(sk, 10)) {
syslog(LOG_ERR, "Can't listen on socket: %s (%d)",
strerror(errno), errno);
goto failed;
}
memset(&scan_event, 0, sizeof(scan_event));
scan_event.events = EPOLLIN;
scan_event.data.fd = sk;
if (epoll_ctl(epoll_fd, EPOLL_CTL_ADD, sk, &scan_event) < 0) {
syslog(LOG_ERR, "Failed to setup scan event watch");
goto failed;
}
vdev.scan_fd = sk;
return 0;
failed:
close(sk);
return 1;
#endif
return data[0];
}
int main(int argc, const char* argv[]) {
int serverL2capSock;
struct sockaddr_l2 sockAddr;
socklen_t sockAddrLen;
int result;
bdaddr_t clientBdAddr;
int clientL2capSock;
fd_set afds;
fd_set rfds;
struct timeval tv;
char stdinBuf[256 * 2 + 1];
char l2capSockBuf[256];
int len;
int i;
struct bt_security btSecurity;
socklen_t btSecurityLen;
uint8_t securityLevel = 0;
// setup signal handlers
signal(SIGINT, signalHandler);
signal(SIGKILL, signalHandler);
signal(SIGHUP, signalHandler);
prctl(PR_SET_PDEATHSIG, SIGINT);
// create socket
serverL2capSock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
// bind
memset(&sockAddr, 0, sizeof(sockAddr));
sockAddr.l2_family = AF_BLUETOOTH;
sockAddr.l2_bdaddr = *BDADDR_ANY;
sockAddr.l2_cid = htobs(ATT_CID);
result = bind(serverL2capSock, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
printf("bind %s\n", (result == -1) ? strerror(errno) : "success");
result = listen(serverL2capSock, 1);
printf("listen %s\n", (result == -1) ? strerror(errno) : "success");
while (result != -1) {
FD_ZERO(&afds);
FD_SET(serverL2capSock, &afds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(serverL2capSock + 1, &afds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
break;
}
} else if (result && FD_ISSET(serverL2capSock, &afds)) {
sockAddrLen = sizeof(sockAddr);
clientL2capSock = accept(serverL2capSock, (struct sockaddr *)&sockAddr, &sockAddrLen);
baswap(&clientBdAddr, &sockAddr.l2_bdaddr);
printf("accept %s\n", batostr(&clientBdAddr));
while(1) {
FD_ZERO(&rfds);
FD_SET(0, &rfds);
FD_SET(clientL2capSock, &rfds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(clientL2capSock + 1, &rfds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal || SIGHUP == lastSignal) {
if (SIGHUP == lastSignal) {
result = 0;
}
break;
}
} else if (result) {
if (FD_ISSET(0, &rfds)) {
len = read(0, stdinBuf, sizeof(stdinBuf));
if (len <= 0) {
break;
}
i = 0;
while(stdinBuf[i] != '\n') {
sscanf(&stdinBuf[i], "%02x", (unsigned int*)&l2capSockBuf[i / 2]);
i += 2;
}
len = write(clientL2capSock, l2capSockBuf, (len - 1) / 2);
}
if (FD_ISSET(clientL2capSock, &rfds)) {
len = read(clientL2capSock, l2capSockBuf, sizeof(l2capSockBuf));
if (len <= 0) {
break;
}
btSecurityLen = sizeof(btSecurity);
memset(&btSecurity, 0, btSecurityLen);
getsockopt(clientL2capSock, SOL_BLUETOOTH, BT_SECURITY, &btSecurity, &btSecurityLen);
if (securityLevel != btSecurity.level) {
securityLevel = btSecurity.level;
const char *securityLevelString;
switch(securityLevel) {
case BT_SECURITY_LOW:
securityLevelString = "low";
break;
case BT_SECURITY_MEDIUM:
securityLevelString = "medium";
break;
case BT_SECURITY_HIGH:
securityLevelString = "high";
break;
default:
securityLevelString = "unknown";
break;
}
printf("security %s\n", securityLevelString);
}
printf("data ");
for(i = 0; i < len; i++) {
printf("%02x", ((int)l2capSockBuf[i]) & 0xff);
}
printf("\n");
}
}
}
printf("disconnect %s\n", batostr(&clientBdAddr));
close(clientL2capSock);
}
}
printf("close\n");
close(serverL2capSock);
return 0;
}
static void cmd_hunt(int dev_id, int argc, char **argv)
{
bdaddr_t bdaddr;
char name[248];
int opt, dd, num=0, num2=0, num3=0, num4=0, num5=0, num6=0;
int btout=50000;
unsigned char lame[16][2] = {"0","1","2","3","4","5","6","7","8","9","A","B","C","D","E","F", };
char addtobrute[248];
printf("redfang - the bluetooth hunter ver 1.00.alpha\n");
printf("(c)2003 \@stake Inc\n");
printf("author: Ollie Whitehouse ([email protected])\n");
argc -= optind;
argv += optind;
if (argc < 2) {
printf(hunt_help);
exit(1);
}
if (argc >= 1) {
btout=atoi(argv[1]);
}
printf("timeout: %d\n", btout);
printf("starting...\n");
while (num <= 15)
{
while(num2 <= 15)
{
while(num3 <= 15)
{
while(num4 <= 15)
{
while(num5 <= 15)
{
while(num6 <= 15)
{
strcpy(addtobrute,"00:80:98:");
strcat(addtobrute,lame[num]);
strcat(addtobrute,lame[num2]);
strcat(addtobrute,":");
strcat(addtobrute,lame[num3]);
strcat(addtobrute,lame[num4]);
strcat(addtobrute,":");
strcat(addtobrute,lame[num5]);
strcat(addtobrute,lame[num6]);
/* debug purposes */
printf("%s\n",addtobrute);
baswap(&bdaddr, strtoba(addtobrute));
dev_id = hci_get_route(&bdaddr);
if (dev_id < 0) {
fprintf(stderr,"Device not availible");
exit(1);
}
dd = hci_open_dev(dev_id);
if (dd < 0) {
fprintf(stderr,"HCI device open failed");
exit(1);
}
/* try to get name of remote device - timeout is the int) */
if (hci_read_remote_name(dd,&bdaddr,sizeof(name), name, btout) == 0)
printf("\n.start--->\naddress :- %s\nname :- %s\n<.end-----\n",batostr(&bdaddr),name);
close(dd);
num6++;
}
num6=0;
num5++;
}
num5=0;
num4++;
}
num4=0;
num3++;
}
num3=0;
num2++;
}
num2=0;
num++;
}
}
int main(int argc, const char* argv[]) {
const char *hciDeviceIdOverride = NULL;
int hciDeviceId = 0;
int hciSocket;
int serverL2capSock;
struct sockaddr_l2 sockAddr;
socklen_t sockAddrLen;
int result;
bdaddr_t clientBdAddr;
int clientL2capSock;
struct l2cap_conninfo l2capConnInfo;
socklen_t l2capConnInfoLen;
int hciHandle;
fd_set afds;
fd_set rfds;
struct timeval tv;
char stdinBuf[256 * 2 + 1];
char l2capSockBuf[256];
int len;
int i;
struct bt_security btSecurity;
socklen_t btSecurityLen;
uint8_t securityLevel = 0;
// remove buffering
setbuf(stdin, NULL);
setbuf(stdout, NULL);
setbuf(stderr, NULL);
// setup signal handlers
signal(SIGINT, signalHandler);
signal(SIGKILL, signalHandler);
signal(SIGHUP, signalHandler);
signal(SIGUSR1, signalHandler);
prctl(PR_SET_PDEATHSIG, SIGINT);
// create socket
serverL2capSock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (argc > 1 && strlen(argv[1]) > 0) {
hciDeviceIdOverride = argv[1];
}
if (hciDeviceIdOverride != NULL) {
hciDeviceId = atoi(hciDeviceIdOverride);
} else {
// if no env variable given, use the first available device
hciDeviceId = hci_get_route(NULL);
}
if (hciDeviceId < 0) {
hciDeviceId = 0; // use device 0, if device id is invalid
}
printf("hciDeviceId %d\n", hciDeviceId);
bdaddr_t daddr;
hciSocket = hci_open_dev(hciDeviceId);
if (hciSocket == -1) {
printf("adapterState unsupported\n");
return -1;
}
if (hci_read_bd_addr(hciSocket, &daddr, 1000) == -1){
daddr = *BDADDR_ANY;
}
printf("bdaddr ");
for(i = 5; i > 0; i--) {
printf("%02x:", daddr.b[i]);
}
printf("%02x", daddr.b[0]);
printf("\n");
// bind
memset(&sockAddr, 0, sizeof(sockAddr));
sockAddr.l2_family = AF_BLUETOOTH;
sockAddr.l2_bdaddr = daddr;
sockAddr.l2_cid = htobs(ATT_CID);
result = bind(serverL2capSock, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
printf("bind %s\n", (result == -1) ? strerror(errno) : "success");
result = listen(serverL2capSock, 1);
printf("listen %s\n", (result == -1) ? strerror(errno) : "success");
while (result != -1) {
FD_ZERO(&afds);
FD_SET(serverL2capSock, &afds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(serverL2capSock + 1, &afds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
break;
} else if (SIGHUP == lastSignal || SIGUSR1 == lastSignal) {
result = 0;
}
} else if (result && FD_ISSET(serverL2capSock, &afds)) {
sockAddrLen = sizeof(sockAddr);
clientL2capSock = accept(serverL2capSock, (struct sockaddr *)&sockAddr, &sockAddrLen);
baswap(&clientBdAddr, &sockAddr.l2_bdaddr);
printf("accept %s\n", batostr(&clientBdAddr));
l2capConnInfoLen = sizeof(l2capConnInfo);
getsockopt(clientL2capSock, SOL_L2CAP, L2CAP_CONNINFO, &l2capConnInfo, &l2capConnInfoLen);
hciHandle = l2capConnInfo.hci_handle;
while(1) {
FD_ZERO(&rfds);
FD_SET(0, &rfds);
FD_SET(clientL2capSock, &rfds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(clientL2capSock + 1, &rfds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
break;
} else if (SIGHUP == lastSignal) {
result = 0;
hci_disconnect(hciSocket, hciHandle, HCI_OE_USER_ENDED_CONNECTION, 1000);
} else if (SIGUSR1 == lastSignal) {
int8_t rssi = 0;
for (i = 0; i < 100; i++) {
hci_read_rssi(hciSocket, hciHandle, &rssi, 1000);
if (rssi != 0) {
break;
}
}
if (rssi == 0) {
rssi = 127;
}
printf("rssi = %d\n", rssi);
}
} else if (result) {
if (FD_ISSET(0, &rfds)) {
len = read(0, stdinBuf, sizeof(stdinBuf));
if (len <= 0) {
break;
}
i = 0;
while(stdinBuf[i] != '\n') {
unsigned int data = 0;
sscanf(&stdinBuf[i], "%02x", &data);
l2capSockBuf[i / 2] = data;
i += 2;
}
len = write(clientL2capSock, l2capSockBuf, (len - 1) / 2);
}
if (FD_ISSET(clientL2capSock, &rfds)) {
len = read(clientL2capSock, l2capSockBuf, sizeof(l2capSockBuf));
if (len <= 0) {
break;
}
btSecurityLen = sizeof(btSecurity);
memset(&btSecurity, 0, btSecurityLen);
getsockopt(clientL2capSock, SOL_BLUETOOTH, BT_SECURITY, &btSecurity, &btSecurityLen);
if (securityLevel != btSecurity.level) {
securityLevel = btSecurity.level;
const char *securityLevelString;
switch(securityLevel) {
case BT_SECURITY_LOW:
securityLevelString = "low";
break;
case BT_SECURITY_MEDIUM:
securityLevelString = "medium";
break;
case BT_SECURITY_HIGH:
securityLevelString = "high";
break;
default:
securityLevelString = "unknown";
break;
}
printf("security %s\n", securityLevelString);
}
printf("data ");
for(i = 0; i < len; i++) {
printf("%02x", ((int)l2capSockBuf[i]) & 0xff);
}
printf("\n");
}
}
}
printf("disconnect %s\n", batostr(&clientBdAddr));
close(clientL2capSock);
}
}
printf("close\n");
close(serverL2capSock);
close(hciSocket);
return 0;
}
static void create_connection(uint8_t *data)
{
create_conn_cp *cp = (void *) data;
struct vhci_link_info info;
struct vhci_conn *conn;
struct sockaddr_in sa;
int h, sk, opt;
bdaddr_t ba;
for (h = 0; h < VHCI_MAX_CONN; h++)
if (!vconn[h])
goto do_connect;
syslog(LOG_ERR, "Too many connections");
return;
do_connect:
if ((sk = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
return;
}
opt = 1;
setsockopt(sk, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt));
baswap(&ba, &vdev.bdaddr);
sa.sin_family = AF_INET;
sa.sin_addr.s_addr = INADDR_ANY; // *(uint32_t *) &ba;
sa.sin_port = 0; // *(uint16_t *) &ba.b[4];
if (bind(sk, (struct sockaddr *) &sa, sizeof(sa))) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
close(sk);
return;
}
baswap(&ba, &cp->bdaddr);
sa.sin_family = AF_INET;
memcpy(&sa.sin_addr.s_addr, &ba, sizeof(sa.sin_addr.s_addr));
sa.sin_port = *(uint16_t *) &ba.b[4];
if (connect(sk, (struct sockaddr *) &sa, sizeof(sa)) < 0) {
syslog(LOG_ERR, "Can't connect: %s (%d)",
strerror(errno), errno);
close(sk);
return;
}
/* Send info */
memset(&info, 0, sizeof(info));
bacpy(&info.bdaddr, &vdev.bdaddr);
info.link_type = ACL_LINK;
info.role = 1;
write_n(sk, (void *) &info, sizeof(info));
if (!(conn = malloc(sizeof(*conn)))) {
syslog(LOG_ERR, "Can't alloc new connection: %s (%d)",
strerror(errno), errno);
close(sk);
return;
}
memcpy((uint8_t *) &ba, (uint8_t *) &sa.sin_addr, 4);
memcpy((uint8_t *) &ba.b[4], (uint8_t *) &sa.sin_port, 2);
baswap(&conn->dest, &ba);
vconn[h] = conn;
conn->handle = h + 1;
conn->chan = g_io_channel_unix_new(sk);
connect_complete(conn);
g_io_add_watch(conn->chan, G_IO_IN | G_IO_NVAL | G_IO_HUP,
io_acl_data, (gpointer) conn);
return;
}
int bu_hci::on_sock_data(uint8_t code, const sdata& buffer) //received
{
uint8_t eventType = buffer.data[0];
uint16_t blen = buffer.len;
std::string scase="NOT HANDLED ";
bybuff trace(buffer.data, buffer.len);
TRACE("{-->["<< int(buffer.len) <<"]"<< trace.to_string());
if (HCI_EVENT_PKT == eventType)
{
uint8_t subEventType = buffer.data[1];
//_TRACE(" Event:" << int(eventType) << ", subevent:" << int(subEventType));
switch(subEventType)
{
case EVT_DISCONN_COMPLETE:
scase="EVT_DISCONN_COMPLETE";
{
evt_disconn_complete* pdc = (evt_disconn_complete*)(buffer.data+4);
pdc->handle = htobs(pdc->handle);
memcpy(&_dcached, pdc, sizeof(_dcached));
#ifdef ACL_MTU_FRAG
flush_acl();
#endif //ACL_MTU_FRAG
_clear();
_pev->on_disconnect(pdc);
_connected=false;
}
break;
case EVT_ENCRYPT_CHANGE:
scase="EVT_ENCRYPT_CHANGE";
{
evt_encrypt_change* pec = (evt_encrypt_change*)(buffer.data+4);
pec->handle=htobs(pec->handle);
_pev->on_encrypt_chnage(pec);
}
break;
case EVT_CMD_COMPLETE:
scase=" [EVT_CMD_COMPLETE]";
{
no_evt_cmd_complete* necc = (no_evt_cmd_complete*)(buffer.data+3);
necc->cmd=htobs(necc->cmd);
this->_oncmd_complette(necc);
}
break;
case EVT_LE_META_EVENT:
scase="EVT_LE_META_EVENT";
{
no_evt_le_meta_event* pev = (no_evt_le_meta_event*)(buffer.data+3);
this->_onmeta(pev);
}
break;
case EVT_CONN_REQUEST:
scase="EVT_CONN_REQUEST";
{
evt_conn_request* preq= (evt_conn_request*)(buffer.data+4);
bdaddr_t dest;
baswap(&dest, &preq->bdaddr);
memcpy(&preq->bdaddr,&dest,sizeof(dest));
}
break;
case EVT_CMD_STATUS: //OCF_AUTH_REQUESTED
scase="EVT_CMD_STATUS";
{
evt_cmd_status* pevs = (evt_cmd_status*)(buffer.data+4);
pevs->opcode = htobs(pevs->opcode);
uint16_t ogf = CMD_OPCODE_OGF(pevs->opcode);
uint16_t ocf = CMD_OPCODE_OCF(pevs->opcode);
TRACE("CMD_STATUS status:" <<int(pevs->status)<<" ncmd:" <<
int(pevs->ncmd) << " opcode(C/G):" <<
std::hex<<int(ocf) <<"/"<<int(ogf) << std::dec);
if(ocf == OCF_EXIT_PERIODIC_INQUIRY)
{
//send_cmd(OCF_INQUIRY_CANCEL, OGF_LINK_CTL,0,0);
}
}
break;
case EVT_REMOTE_NAME_REQ_COMPLETE:
scase="EVT_REMOTE_NAME_REQ_COMPLETE";
{
evt_remote_name_req_complete* pnc = (evt_remote_name_req_complete*)(buffer.data+4);
TRACE("remote name: " << pnc->name);
}
break;
#ifdef ACL_MTU_FRAG
case EVT_NUM_COMP_PKTS:
scase="EVT_NUM_COMP_PKTS";
{
uint8_t nhandles = uint8_t(buffer.data[3]);
// TRACE("GOT number of completted acl packets:" << int(nhandles));
for(uint8_t h=0; h<nhandles; h++)
{
no_acl_handler_packet* pconfirm = (no_acl_handler_packet*)(buffer.data + 4 + (h*4));
pconfirm->handler = htobs(pconfirm->handler);
pconfirm->packet = htobs(pconfirm->packet);
// TRACE("GOT Pending handler:" << int(pconfirm->handler) << ", " << int(pconfirm->packet));
const auto& ah = _aclPending.find(pconfirm->handler);
if(ah == this->_aclPending.end())
{
// TRACE("HANDLER "<< int(pconfirm->handler) <<" NOT FOUND");
continue;
}
else
{
ah->second -= pconfirm->packet;
if(ah->second <= 0)
{
// TRACE("DELETE ALL HANDLER packets:" <<",["<<int(pconfirm->handler) <<"]" << int(ah->second));
_erase_AclOut(ah->first);
}
}
}
this->flush_acl();
}
break;
#endif //ACL_MTU_FRAG
default:
break;
}//switch
}
else if (HCI_ACLDATA_PKT == eventType)
{
uint16_t val = oa2t<uint16_t>(buffer.data,1);
uint16_t flags = acl_flags(val);
uint16_t handle = acl_handle(val);
if(ACL_START_NO_FLUSH == flags)
{
//_clear_aclInQueue(); TODO
scase="ACL_START_NO_FLUSH";
flags = ACL_START;
}
if (ACL_START == flags)
{
uint16_t cid = oa2t<uint16_t>(buffer.data,7);
uint16_t expectedlen = oa2t<uint16_t>(buffer.data,5);
uint16_t chunklen = blen-9;
sdata sd;
scase="ACL_START";
sd.len = expectedlen;
sd.data = buffer.data + 9;
if (expectedlen == chunklen)
{
_pev->on_acl_packet(handle,cid, sd);
}
else
{
/// assert(_aclIn.find(handle) == _aclIn.end());
//accumulate data bt handler
if(_aclIn.find(handle) == _aclIn.end())
{
no_acl_start_len_dynamic* pd = new(no_acl_start_len_dynamic);
pd->cit = (cid);
pd->len = (chunklen);
pd->expectedlen = (expectedlen);
pd->byarr.insert(pd->byarr.end(), sd.data, sd.data + sd.len);
_aclIn[handle] = pd;
}
else
{
_TRACE("ERROR AHNDLER NOT FOUND ....!. \n" << handle);
}
}
}
else if (ACL_CONT == flags)
{
/// keep accumulating, or discard
uint16_t chunklen = blen-9;
sdata sd;
scase="ACL_CONT";
auto el = _aclIn.find(handle);
if (el == _aclIn.end())
return true;
no_acl_start_len_dynamic* pd = el->second;
pd->len += chunklen;
pd->byarr.insert(pd->byarr.end(), sd.data, sd.data + sd.len);
//assert(pd->expectedlen == expectedlen);
if(pd->expectedlen == pd->len )
{
sd.len = pd->len;
sd.data = &pd->byarr[0];
_pev->on_acl_packet(handle, pd->cit, sd);
delete pd;
_aclIn.erase(handle);
}
}
else
{
_TRACE("!!! HCI_ACLDATA_PKT unknown flag: " << std::hex << int(flags) << std::dec);
}
}
else{
_TRACE("!!! NO KNOWN on_sock_data EVENTTYPE " << std::hex << int(eventType) << std::dec );
}
//TRACE("HCI: " << scase << " }");
return true;
}
static void hciDiscovery(int dev_id)
{
int length, flags, dd, i;
bdaddr_t bdaddr;
char name[248];
inquiry_info *info = NULL;
int num_rsp = 100;
dd = hci_open_dev(dev_id);
if (dd < 0)
{
fprintf(stderr, "HCI device open failed");
free(info);
exit(1);
}
int ctl;
if ((ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI)) < 0)
{
perror("Can't open HCI socket.");
}
struct hci_dev_req dr;
dr.dev_id = dev_id;
dr.dev_opt = SCAN_DISABLED;
// Stop the dongles from discovering each other!
ioctl(ctl, HCISETSCAN, (unsigned long) &dr);
/* Reset HCI device with a stop[ and start*/
printf("resetting hci device. \n");
ioctl(ctl, HCIDEVDOWN, dev_id);
ioctl(ctl, HCIDEVUP, dev_id);
close(ctl);
// Need to field test small values.
length = 4; /* multiply by 1.2 seconds */
flags = IREQ_CACHE_FLUSH;
printf("sending inquiry\n");
num_rsp = hci_inquiry(dev_id, length, num_rsp, NULL, &info, flags);
if(num_rsp > 0)
{
printf("hci%d saw %d responses\n", dev_id, num_rsp);
}
// we need to come up with a device class lookup function
// this is just incase we drive or walk by too quick to get a devname... this should be logged somewhere.
char addr[18];
for (i = 0; i < num_rsp; i++)
{
ba2str(&(info+i)->bdaddr, addr);
printf("%s\tclass: 0x%2.2x%2.2x%2.2x\n",addr,
(info+i)->dev_class[2],
(info+i)->dev_class[1],
(info+i)->dev_class[0]);
}
// this may need to be done differently.
for(i = 0; i < num_rsp; i++)
{
memset(name, 0, sizeof(name));
if (hci_read_remote_name(dd, &(info+i)->bdaddr, sizeof(name), name, opts.btout) == 0)
{
baswap(&bdaddr, &(info+i)->bdaddr);
printf("hci%d Discovered: %s [%s] - %s\n", dev_id, name, batostr(&bdaddr), findManf(batostr(&bdaddr)));
sdptool(name,findManf(batostr(&bdaddr)),batostr(&bdaddr),dev_id);
}
}
// where does the sdptool code go?
hci_close_dev(dd);
free(info);
}
