git.gag.com Git - fw/altos/blob - libaltos/btletest.c

   1 /*
   2  * Copyright © 2017 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  */
  14 #define _GNU_SOURCE
  15
  16 #include <stdio.h>
  17 #include <stdlib.h>
  18 #include <string.h>
  19 #include <fcntl.h>
  20 #include <termios.h>
  21 #include <errno.h>
  22 #include <unistd.h>
  23 #include <ctype.h>
  24 #include <dirent.h>
  25 #include <bluetooth/bluetooth.h>
  26 #include <bluetooth/hci.h>
  27 #include <bluetooth/hci_lib.h>
  28 #include <bluetooth/rfcomm.h>
  29 #include <bluetooth/sdp.h>
  30 #include <bluetooth/sdp_lib.h>
  31 #include <bluetooth/l2cap.h>
  32 #include <poll.h>
  33
  34 #define ATT_OP_MTU_REQ          0x02
  35 #define ATT_OP_MTU_RESP         0x03
  36 #define ATT_OP_WRITE_CMD        0x52
  37 #define ATT_OP_HANDLE_NOTIFY    0x1b
  38 #define CID_ATT                 0x0004
  39 #define TX_ENDPOINT             0x003a
  40 #define RX_ENDPOINT             0x0037
  41 #define RX_NOTIFY               0x0038
  42
  43 int
  44 main(int argc, char **argv)
  45 {
  46         int sk;
  47         int psm;
  48         struct sockaddr_l2 src_addr = { 0 };
  49         struct sockaddr_l2 dst_addr = { 0 };
  50         char buf[1024];
  51         struct pollfd   fd[2];
  52         int n, i;
  53         char *btaddr;
  54         int     mtu;
  55
  56         btaddr = argc > 1 ? argv[1] : "D8:80:39:F3:4E:A5";
  57
  58         sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
  59         if (sk < 0) {
  60                 perror("socket");
  61                 exit(1);
  62         }
  63
  64         src_addr.l2_family = AF_BLUETOOTH;
  65         /* Leave src_addr.l2_bdaddr all zeros */
  66         src_addr.l2_cid = htobs(CID_ATT);
  67         src_addr.l2_bdaddr_type = BDADDR_LE_PUBLIC;
  68         if (bind(sk, (struct sockaddr *) &src_addr, sizeof (src_addr)) < 0) {
  69                 perror("bind");
  70                 exit(1);
  71         }
  72
  73         dst_addr.l2_family = AF_BLUETOOTH;
  74         str2ba(btaddr, &dst_addr.l2_bdaddr);
  75         dst_addr.l2_cid = htobs(CID_ATT);
  76         dst_addr.l2_bdaddr_type = BDADDR_LE_PUBLIC;
  77
  78         if (connect(sk, (struct sockaddr *) &dst_addr, sizeof(dst_addr)) < 0) {
  79                 perror("connect");
  80                 exit(1);
  81         }
  82
  83         buf[0] = ATT_OP_MTU_REQ;
  84         buf[1] = sizeof(buf) & 0xff;
  85         buf[2] = sizeof(buf) >> 8;
  86         n = write(sk, buf, 3);
  87         if (n != 3) {
  88                 perror("write mtu\n");
  89                 exit(1);
  90         }
  91
  92         fd[0].fd = sk;
  93         fd[0].events = POLLIN;
  94         for (;;) {
  95                 n = poll(fd, 1, 3000);
  96                 if (n <= 0) {
  97                         printf("timeout waiting for MTU response\n");
  98                         exit(1);
  99                 }
 100                 if (fd[0].revents & POLLIN) {
 101                         n = read(sk, buf, sizeof(buf));
 102                         printf("read %d\n", n);
 103                         for (i = 0; i < n; i++)
 104                                 printf("%02x\n", buf[i]);
 105                         if (buf[0] == ATT_OP_MTU_RESP) {
 106                                 mtu = (buf[1] & 0xff) | ((buf[2] & 0xff) << 8);
 107                                 break;
 108                         }
 109                 }
 110         }
 111         printf("mtu %d\n", mtu);
 112
 113         buf[0] = ATT_OP_WRITE_CMD;
 114         buf[1] = RX_NOTIFY & 0xff;
 115         buf[2] = RX_NOTIFY >> 8;
 116         buf[3] = 1;
 117         n = write(sk, buf, 4);
 118         if (n != 4) {
 119                 perror("write notify");
 120                 exit(1);
 121         }
 122
 123         fd[0].fd = 0;
 124         fd[0].events = POLLIN;
 125         fd[1].fd = sk;
 126         fd[1].events = POLLIN;
 127
 128         for (;;) {
 129                 n = poll(fd, 2, -1);
 130                 if (n == 0)
 131                         continue;
 132                 if (fd[0].revents & POLLIN) {
 133                         char    *b;
 134                         n = read(0, buf+3, sizeof(buf)-3);
 135                         if (n < 0) {
 136                                 perror("read stdin");
 137                                 exit(1);
 138                         }
 139                         if (n == 0)
 140                                 break;
 141
 142                         b = buf;
 143                         while (n > 0) {
 144                                 int this = n;
 145                                 if (this > mtu - 3)
 146                                         this = mtu - 3;
 147
 148                                 b[0] = ATT_OP_WRITE_CMD;
 149                                 b[1] = TX_ENDPOINT;
 150                                 b[2] = TX_ENDPOINT >> 8;
 151                                 if (write(sk, b, this + 3) != this + 3) {
 152                                         perror("write sk");
 153                                         exit(1);
 154                                 }
 155                                 b += this;
 156                                 n -= this;
 157                         }
 158                 }
 159                 if (fd[1].revents & POLLIN) {
 160                         uint16_t        ch;
 161
 162                         n = read(sk, buf, sizeof(buf));
 163                         if (n < 0) {
 164                                 perror("read sk");
 165                                 exit(1);
 166                         }
 167                         if (n == 0)
 168                                 continue;
 169                         ch = buf[1] | (buf[2] << 8);
 170                         switch (buf[0]) {
 171                         case ATT_OP_HANDLE_NOTIFY:
 172                                 if (ch == RX_ENDPOINT)
 173                                         write(1, buf+3, n-3);
 174                                 break;
 175                         }
 176                 }
 177                 if (fd[1].revents & (POLLERR|POLLHUP))
 178                         break;
 179         }
 180         close(sk);
 181
 182         return 0;
 183 }