diff --git a/tools/btgatt-server.c b/tools/btgatt-server.c
index c0d8dd6..c724ab5 100644
--- a/tools/btgatt-server.c
+++ b/tools/btgatt-server.c
#include <stdint.h>
#include <stdlib.h>
#include <getopt.h>
+#include <unistd.h>
#include <bluetooth/bluetooth.h>
#include <bluetooth/hci.h>
#include <bluetooth/l2cap.h>
#include "lib/uuid.h"
+#include "monitor/mainloop.h"
+#include "src/shared/util.h"
+#include "src/shared/att.h"
+#include "src/shared/queue.h"
+#include "src/shared/gatt-db.h"
+#include "src/shared/gatt-server.h"
+
+#define ATT_CID 4
+
+#define PRLOG(...) \
+ do { \
+ printf(__VA_ARGS__); \
+ print_prompt(); \
+ } while (0)
+
+#define COLOR_OFF "\x1B[0m"
+#define COLOR_RED "\x1B[0;91m"
+#define COLOR_GREEN "\x1B[0;92m"
+#define COLOR_YELLOW "\x1B[0;93m"
+#define COLOR_BLUE "\x1B[0;94m"
+#define COLOR_MAGENTA "\x1B[0;95m"
+#define COLOR_BOLDGRAY "\x1B[1;30m"
+#define COLOR_BOLDWHITE "\x1B[1;37m"
+
static bool verbose = false;
+struct server {
+ int fd;
+ struct gatt_db *db;
+ struct bt_gatt_server *gatt;
+};
+
+static void print_prompt(void)
+{
+ printf(COLOR_BLUE "[GATT server]" COLOR_OFF "# ");
+ fflush(stdout);
+}
+
+static void att_disconnect_cb(void *user_data)
+{
+ printf("Device disconnected\n");
+
+ mainloop_quit();
+}
+
+static void att_debug_cb(const char *str, void *user_data)
+{
+ const char *prefix = user_data;
+
+ PRLOG(COLOR_BOLDGRAY "%s" COLOR_BOLDWHITE "%s\n" COLOR_OFF, prefix,
+ str);
+}
+
+static void gatt_debug_cb(const char *str, void *user_data)
+{
+ const char *prefix = user_data;
+
+ PRLOG(COLOR_GREEN "%s%s\n" COLOR_OFF, prefix, str);
+}
+
+static struct server *server_create(int fd, uint16_t mtu)
+{
+ struct server *server;
+ struct bt_att *att;
+
+ server = new0(struct server, 1);
+ if (!server) {
+ fprintf(stderr, "Failed to allocate memory for server\n");
+ return NULL;
+ }
+
+ att = bt_att_new(fd);
+ if (!att) {
+ fprintf(stderr, "Failed to initialze ATT transport layer\n");
+ goto fail;
+ }
+
+ if (!bt_att_set_close_on_unref(att, true)) {
+ fprintf(stderr, "Failed to set up ATT transport layer\n");
+ goto fail;
+ }
+
+ if (!bt_att_register_disconnect(att, att_disconnect_cb, NULL, NULL)) {
+ fprintf(stderr, "Failed to set ATT disconnect handler\n");
+ goto fail;
+ }
+
+ server->fd = fd;
+ server->db = gatt_db_new();
+ if (!server->db) {
+ fprintf(stderr, "Failed to create GATT database\n");
+ goto fail;
+ }
+
+ server->gatt = bt_gatt_server_new(server->db, att, mtu);
+ if (!server->gatt) {
+ fprintf(stderr, "Failed to create GATT server\n");
+ goto fail;
+ }
+
+ if (verbose) {
+ bt_att_set_debug(att, att_debug_cb, "att: ", NULL);
+ bt_gatt_server_set_debug(server->gatt, gatt_debug_cb,
+ "server: ", NULL);
+ }
+
+ /* bt_gatt_server already holds a reference */
+ bt_att_unref(att);
+
+ return server;
+
+fail:
+ gatt_db_destroy(server->db);
+ bt_att_unref(att);
+ free(server);
+
+ return NULL;
+}
+
+static void server_destroy(struct server *server)
+{
+ bt_gatt_server_unref(server->gatt);
+ gatt_db_destroy(server->db);
+}
+
static void usage(void)
{
printf("btgatt-server\n");
{ }
};
+static int l2cap_le_att_listen_and_accept(bdaddr_t *src, int sec)
+{
+ int sk, nsk;
+ struct sockaddr_l2 srcaddr, addr;
+ socklen_t optlen;
+ struct bt_security btsec;
+ char ba[18];
+
+ sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
+ if (sk < 0) {
+ perror("Failed to create L2CAP socket");
+ return -1;
+ }
+
+ /* Set up source address */
+ memset(&srcaddr, 0, sizeof(srcaddr));
+ srcaddr.l2_family = AF_BLUETOOTH;
+ srcaddr.l2_cid = htobs(ATT_CID);
+ srcaddr.l2_bdaddr_type = 0;
+ bacpy(&srcaddr.l2_bdaddr, src);
+
+ if (bind(sk, (struct sockaddr *) &srcaddr, sizeof(srcaddr)) < 0) {
+ perror("Failed to bind L2CAP socket");
+ goto fail;
+ }
+
+ /* Set the security level */
+ memset(&btsec, 0, sizeof(btsec));
+ btsec.level = sec;
+ if (setsockopt(sk, SOL_BLUETOOTH, BT_SECURITY, &btsec,
+ sizeof(btsec)) != 0) {
+ fprintf(stderr, "Failed to set L2CAP security level\n");
+ goto fail;
+ }
+
+ if (listen(sk, 10) < 0) {
+ perror("Listening on socket failed");
+ goto fail;
+ }
+
+ printf("Started listening on ATT channel. Waiting for connections\n");
+
+ memset(&addr, 0, sizeof(addr));
+ optlen = sizeof(addr);
+ nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
+ if (nsk < 0) {
+ perror("Accept failed");
+ goto fail;
+ }
+
+ ba2str(&addr.l2_bdaddr, ba);
+ printf("Connect from %s\n", ba);
+ close(sk);
+
+ return nsk;
+
+fail:
+ close(sk);
+ return -1;
+}
+
int main(int argc, char *argv[])
{
int opt;
bdaddr_t src_addr;
int dev_id = -1;
+ int fd;
+ int sec = BT_SECURITY_LOW;
+ uint16_t mtu = 0;
+ struct server *server;
while ((opt = getopt_long(argc, argv, "+hvs:m:i:",
main_options, NULL)) != -1) {
verbose = true;
break;
case 's':
- /* TODO */
+ if (strcmp(optarg, "low") == 0)
+ sec = BT_SECURITY_LOW;
+ else if (strcmp(optarg, "medium") == 0)
+ sec = BT_SECURITY_MEDIUM;
+ else if (strcmp(optarg, "high") == 0)
+ sec = BT_SECURITY_HIGH;
+ else {
+ fprintf(stderr, "Invalid security level\n");
+ return EXIT_FAILURE;
+ }
break;
- case 'm':
- /* TODO */
+ case 'm': {
+ int arg;
+
+ arg = atoi(optarg);
+ if (arg <= 0) {
+ fprintf(stderr, "Invalid MTU: %d\n", arg);
+ return EXIT_FAILURE;
+ }
+
+ if (arg > UINT16_MAX) {
+ fprintf(stderr, "MTU too large: %d\n", arg);
+ return EXIT_FAILURE;
+ }
+
+ mtu = (uint16_t) arg;
break;
+ }
case 'i':
dev_id = hci_devid(optarg);
if (dev_id < 0) {
return EXIT_FAILURE;
}
- /* TODO: Set up mainloop and listening LE socket */
+ fd = l2cap_le_att_listen_and_accept(&src_addr, sec);
+ if (fd < 0) {
+ fprintf(stderr, "Failed to accept L2CAP ATT connection\n");
+ return EXIT_FAILURE;
+ }
+
+ mainloop_init();
+
+ server = server_create(fd, mtu);
+ if (!server) {
+ close(fd);
+ return EXIT_FAILURE;
+ }
+
+ printf("Running GATT server\n");
+
+ mainloop_run();
+
+ printf("\n\nShutting down...\n");
+
+ server_destroy(server);
- return 0;
+ return EXIT_SUCCESS;
}