bt-oal/bluez_hal/src/bt-hal-l2cap-le-dbus-handler.c

   1 /*
   2  * Bluetooth-frwk
   3  *
   4  * Copyright (c) 2000 - 2011 Samsung Electronics Co., Ltd. All rights reserved.
   5  *
   6  * Licensed under the Apache License, Version 2.0 (the "License");
   7  * you may not use this file except in compliance with the License.
   8  * You may obtain a copy of the License at
   9  *
  10  *              http://www.apache.org/licenses/LICENSE-2.0
  11  *
  12  * Unless required by applicable law or agreed to in writing, software
  13  * distributed under the License is distributed on an "AS IS" BASIS,
  14  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  15  * See the License for the specific language governing permissions and
  16  * limitations under the License.
  17  *
  18  */
  19
  20 #include <stdio.h>
  21 #include <stdlib.h>
  22 #include <stdbool.h>
  23 #include <string.h>
  24 #include <sys/types.h>
  25 #include <sys/socket.h>
  26
  27 #include <glib.h>
  28 #include <gio/gio.h>
  29 #include <dlog.h>
  30
  31 #include "bt-hal-l2cap-le-dbus-handler.h"
  32 #include "bt-hal-dbus-common-utils.h"
  33 #include "bt-hal-socket-common.h"
  34
  35 #define BT_HAL_L2CAP_LE_ID_MAX 245
  36 #define BT_HAL_L2CAP_LE_MAX_BUFFER_SIZE 0xFFFF
  37 #define L2CAP_LE_DEFAULT_CHANNEL 0
  38 #define L2CAP_LE_PSM_MAX 0xFFFF
  39 #define L2CAP_LE_UUID_SUBSTR "FFFFFFFF-FFFF-FFFF-FFFF-"
  40
  41 typedef struct {
  42         int psm;
  43         char *obj_path;
  44         int object_id;
  45         int id;
  46         int server_fd;
  47         unsigned int server_watch;
  48         GSList *conn_list;
  49 } l2cap_le_server_data_t;
  50
  51 static GSList *l2cap_le_clients;
  52 static GSList *l2cap_le_servers;
  53 static int latest_id = -1;
  54 static gboolean id_used[BT_HAL_L2CAP_LE_ID_MAX];
  55
  56 static int __l2cap_le_assign_id(void)
  57 {
  58         int index;
  59
  60         DBG("latest_id: %d", latest_id);
  61
  62         index = latest_id + 1;
  63         if (index >= BT_HAL_L2CAP_LE_ID_MAX)
  64                 index = 0;
  65
  66         DBG("index: %d", index);
  67
  68         while (id_used[index] == TRUE) {
  69                 if (index == latest_id) {
  70                         ERR("All request ID is used");
  71                         return -1;
  72                 }
  73
  74                 index++;
  75                 if (index >= BT_HAL_L2CAP_LE_ID_MAX)
  76                         index = 0;
  77         }
  78
  79         latest_id = index;
  80         id_used[index] = TRUE;
  81         INFO("Assigned Id: %d", latest_id);
  82
  83         return latest_id;
  84 }
  85
  86 static void __l2cap_le_delete_id(int id)
  87 {
  88         if (id >= BT_HAL_L2CAP_LE_ID_MAX || id < 0) {
  89                 ERR("Invalid id %d", id);
  90                 return;
  91         }
  92
  93         id_used[id] = FALSE;
  94         latest_id = id - 1;
  95         DBG("id: %d, latest_id: %d", id, latest_id);
  96 }
  97
  98 /*************************** L2CAP_LE Client Implementation ***************************/
  99 l2cap_le_cb_data_t *__find_l2cap_le_client_info_from_path(const char *path)
 100 {
 101         GSList *l;
 102
 103         for (l = l2cap_le_clients; l != NULL; l = l->next) {
 104                 l2cap_le_cb_data_t *info = l->data;
 105
 106                 if (info != NULL)
 107                         if (g_strcmp0(info->obj_path, path) == 0)
 108                                 return info;
 109         }
 110
 111         return NULL;
 112 }
 113
 114 static void __bt_free_conn(l2cap_le_conn_info_t *conn)
 115 {
 116         if (conn == NULL)
 117                 return;
 118
 119         if (0 < conn->hal_fd) {
 120                 shutdown(conn->hal_fd, SHUT_RDWR);
 121                 close(conn->hal_fd);
 122         }
 123
 124         if (conn->hal_watch > 0) {
 125                 g_source_remove(conn->hal_watch);
 126                 conn->hal_watch = 0;
 127         }
 128
 129         if (0 < conn->stack_fd) {
 130                 shutdown(conn->stack_fd, SHUT_RDWR);
 131                 close(conn->stack_fd);
 132         }
 133
 134         if (conn->bt_watch > 0) {
 135                 g_source_remove(conn->bt_watch);
 136                 conn->bt_watch = 0;
 137         }
 138
 139         g_free(conn);
 140 }
 141
 142 static void __bt_free_cb_data(l2cap_le_cb_data_t *cb_data)
 143 {
 144         DBG("+");
 145
 146         if (cb_data->id >= 0)
 147                 __l2cap_le_delete_id(cb_data->id);
 148
 149         if (cb_data->object_id > 0)
 150                 _bt_hal_unregister_gdbus_object(cb_data->object_id);
 151
 152         if (cb_data->obj_path) {
 153                 INFO("remove l2cap_le socket");
 154                 _bt_hal_remove_l2cap_le_socket(cb_data->obj_path);
 155         }
 156
 157         g_free(cb_data->obj_path);
 158         g_free(cb_data->device_path);
 159         g_free(cb_data);
 160
 161         DBG("-");
 162 }
 163
 164 void __l2cap_le_cb_data_remove(l2cap_le_cb_data_t *info)
 165 {
 166         if (!info) {
 167                 ERR("info is NULL");
 168                 return;
 169         }
 170
 171         l2cap_le_clients = g_slist_remove(l2cap_le_clients, info);
 172         __bt_free_conn(info->conn_info);
 173         __bt_free_cb_data(info);
 174 }
 175
 176 static int write_all(int fd, unsigned char *buf, int len)
 177 {
 178         int sent = 0;
 179         DBG("len %d", len);
 180
 181         while (len > 0) {
 182                 int written;
 183
 184                 written = write(fd, buf, len);
 185                 if (written < 0) {
 186                         if (errno == EINTR || errno == EAGAIN)
 187                                 continue;
 188                         return -1;
 189                 }
 190
 191                 if (!written)
 192                         return 0;
 193
 194                 len -= written; buf += written; sent += written;
 195         }
 196
 197         return sent;
 198 }
 199
 200 static gboolean app_event_cb(GIOChannel *io, GIOCondition cond, gpointer data)
 201 {
 202         gsize len;
 203         int sent;
 204         l2cap_le_cb_data_t *info = data;
 205         l2cap_le_conn_info_t *conn_info;
 206         unsigned char buff[BT_HAL_L2CAP_LE_MAX_BUFFER_SIZE];
 207         GError *err = NULL;
 208         int fd;
 209         char err_msg[256] = {0, };
 210
 211         DBG("+");
 212         fd = g_io_channel_unix_get_fd(io);
 213         conn_info = info->conn_info;
 214
 215         if (cond & G_IO_HUP) {
 216                 ERR("Socket %d hang up", fd);
 217                 goto fail;
 218         }
 219
 220         if (cond & (G_IO_ERR | G_IO_NVAL)) {
 221                 ERR("Socket %d error", fd);
 222                 goto fail;
 223         }
 224
 225         if (!conn_info) {
 226                 ERR("conn_info is NULL");
 227                 return TRUE;
 228         }
 229
 230         /* Read data from application */
 231         if (g_io_channel_read_chars(io, (gchar *)buff,
 232                         BT_HAL_L2CAP_LE_MAX_BUFFER_SIZE,
 233                         &len, &err) == G_IO_STATUS_ERROR) {
 234                 if (err)
 235                         ERR("IO Channel read error: %s", err->message);
 236                 else
 237                         ERR("IO Channel read error client");
 238                 goto fail;
 239         }
 240
 241         INFO("len: %zu", len);
 242         if (0 == len) {
 243                 ERR("Other end of socket is closed");
 244                 goto fail;
 245         }
 246
 247         /* Send data to remote device */
 248         sent = write_all(conn_info->stack_fd, buff, len);
 249         if (sent < 0) {
 250                 strerror_r(errno, err_msg, sizeof(err_msg));
 251                 ERR("write(): %s", err_msg);
 252                 goto fail;
 253         }
 254
 255         DBG("-");
 256         return TRUE;
 257 fail:
 258         __l2cap_le_cb_data_remove(info);
 259         return FALSE;
 260 }
 261
 262 static gboolean stack_event_cb(GIOChannel *io, GIOCondition cond, gpointer data)
 263 {
 264         unsigned int len;
 265         int sent;
 266         l2cap_le_cb_data_t *info = data;
 267         l2cap_le_conn_info_t *conn_info;
 268         unsigned char buff[BT_HAL_L2CAP_LE_MAX_BUFFER_SIZE];
 269         GError *err = NULL;
 270         int fd;
 271         char err_msg[256] = {0, };
 272
 273         DBG("+");
 274
 275         fd = g_io_channel_unix_get_fd(io);
 276         conn_info = info->conn_info;
 277
 278         if (cond & G_IO_HUP) {
 279                 ERR("Socket %d hang up", fd);
 280                 goto fail;
 281         }
 282
 283         if (cond & (G_IO_ERR | G_IO_NVAL)) {
 284                 ERR("Socket %d error", fd);
 285                 goto fail;
 286         }
 287
 288         if (!conn_info) {
 289                 ERR("conn_info is NULL");
 290                 return TRUE;
 291         }
 292
 293         /* Read data from remote device */
 294         if (g_io_channel_read_chars(io, (gchar *)buff,
 295                         BT_HAL_L2CAP_LE_MAX_BUFFER_SIZE,
 296                         (gsize *)&len, &err) == G_IO_STATUS_ERROR) {
 297                 if (err)
 298                         ERR("IO Channel read error: %s", err->message);
 299                 else
 300                         ERR("IO Channel read error client");
 301                 goto fail;
 302         }
 303
 304         INFO("len: %d", len);
 305         if (0 == len) {
 306                 ERR("Other end of socket is closed");
 307                 goto fail;
 308         }
 309
 310         /* Send data to application */
 311         sent = write_all(conn_info->hal_fd, buff, len);
 312         if (sent < 0) {
 313                 strerror_r(errno, err_msg, sizeof(err_msg));
 314                 ERR("write(): %s", err_msg);
 315                 goto fail;
 316         }
 317
 318         DBG("-");
 319         return TRUE;
 320 fail:
 321         __l2cap_le_cb_data_remove(info);
 322         return FALSE;
 323 }
 324
 325 static int __new_connection(const char *path, int fd, bt_bdaddr_t *addr)
 326 {
 327         char address[BT_HAL_ADDRESS_STRING_SIZE];
 328         l2cap_le_cb_data_t *info;
 329         l2cap_le_conn_info_t *conn_info;
 330         struct hal_ev_sock_connect ev;
 331         GIOCondition cond;
 332         int len;
 333         GIOChannel *io;
 334         char err_msg[256] = {0, };
 335
 336         /* TODO: Temperary, later need to fill correct psm */
 337         int psm = 0;
 338
 339         if (NULL == path || NULL == addr) {
 340                 ERR("NULL == path || NULL = addr");
 341                 return -1;
 342         }
 343
 344         info = __find_l2cap_le_client_info_from_path(path);
 345         if (info == NULL) {
 346                 ERR("info is null");
 347                 return -1;
 348         }
 349
 350         conn_info = info->conn_info;
 351         _bt_hal_convert_addr_type_to_string(address, addr->address);
 352         if (conn_info == NULL) {
 353                 ERR("conn_info is NULL for dev:[%s]", address);
 354                 return -1;
 355         }
 356
 357         if (write(conn_info->hal_fd, &psm, sizeof(psm)) != sizeof(psm)) {
 358                 ERR("Error sending l2cap_le channel");
 359                 goto fail;
 360         }
 361
 362         conn_info->stack_fd = fd;
 363         DBG("Remote address: %s, l2cap_le stack fd: %d, hal_fd %d",
 364                         address, conn_info->stack_fd, conn_info->hal_fd);
 365
 366         /* Send l2cap_le connected event */
 367         memset(&ev, 0, sizeof(ev));
 368         ev.size = sizeof(ev);
 369         memcpy(ev.bdaddr, addr->address, 6);
 370         ev.status = BT_STATUS_SUCCESS;
 371
 372         // TODO: Need to fill corrrect psm later on
 373         ev.channel = psm;
 374
 375         len = write_all(conn_info->hal_fd, (unsigned char *)&ev, sizeof(ev));
 376         if (len < 0) {
 377                 strerror_r(errno, err_msg, sizeof(err_msg));
 378                 ERR("%s", err_msg);
 379                 goto fail;
 380         }
 381
 382         if (len != sizeof(ev)) {
 383                 ERR("Error sending connect event");
 384                 goto fail;;
 385         }
 386
 387         /* Handle events from App */
 388         cond = G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL;
 389         io = g_io_channel_unix_new(conn_info->hal_fd);
 390         conn_info->hal_watch = g_io_add_watch(io, cond, app_event_cb, info);
 391         g_io_channel_set_flags(io, G_IO_FLAG_NONBLOCK, NULL);
 392         g_io_channel_set_encoding(io, NULL, NULL);
 393         g_io_channel_unref(io);
 394
 395         /* Handle l2cap_le events from bluez */
 396         cond = G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
 397         io = g_io_channel_unix_new(conn_info->stack_fd);
 398         conn_info->bt_watch = g_io_add_watch(io, cond, stack_event_cb, info);
 399         g_io_channel_set_flags(io, G_IO_FLAG_NONBLOCK, NULL);
 400         g_io_channel_set_encoding(io, NULL, NULL);
 401         g_io_channel_unref(io);
 402
 403         return 0;
 404 fail:
 405         __l2cap_le_cb_data_remove(info);
 406         return -1;
 407 }
 408
 409 static void __bt_connect_response_cb(GDBusProxy *proxy, GAsyncResult *res,
 410                                                 gpointer user_data)
 411 {
 412         GError *error = NULL;
 413         l2cap_le_cb_data_t *cb_data;
 414         GVariant *result;
 415
 416         DBG("+");
 417
 418         cb_data = user_data;
 419         if (cb_data == NULL) {
 420                 ERR("cb_data == NULL");
 421                 return;
 422         }
 423
 424         result = g_dbus_proxy_call_finish(proxy, res, &error);
 425         if (!result) {
 426                 ERR("Error : %s \n", error->message);
 427                 __l2cap_le_cb_data_remove(cb_data);
 428                 g_error_free(error);
 429         } else {
 430                 g_variant_unref(result);
 431         }
 432
 433         if (proxy)
 434                 g_object_unref(proxy);
 435
 436         DBG("-");
 437 }
 438
 439 static l2cap_le_cb_data_t *__get_l2cap_le_cb_data(int psm)
 440 {
 441         int id;
 442         int object_id;
 443         char *path;
 444         l2cap_le_cb_data_t *cb_data;
 445
 446         DBG("+");
 447
 448         id = __l2cap_le_assign_id();
 449         if (id < 0) {
 450                 ERR("__l2cap_le_assign_id failed");
 451                 return NULL;
 452         }
 453
 454         path = g_strdup_printf("/org/socket/client/%d/%d", getpid(), id);
 455         object_id = _bt_hal_register_new_l2cap_le_gdbus_object(path, __new_connection);
 456         if (object_id < 0) {
 457                 ERR("_bt_hal_register_new_l2cap_le_gdbus_object failed");
 458                 g_free(path);
 459                 __l2cap_le_delete_id(id);
 460                 return NULL;
 461         }
 462
 463         cb_data = g_malloc0(sizeof(l2cap_le_cb_data_t));
 464         cb_data->psm = psm;
 465         cb_data->obj_path = path;
 466         cb_data->object_id = object_id;
 467         cb_data->id = id;
 468
 469         DBG("-");
 470         return cb_data;
 471 }
 472
 473 static l2cap_le_conn_info_t *__l2cap_le_create_conn_info(char *addr, int *sock)
 474 {
 475         int fds[2] = {-1, -1};
 476         l2cap_le_conn_info_t *conn;
 477         char err_msg[256] = {0, };
 478
 479         if (socketpair(AF_UNIX, SOCK_STREAM, 0, fds) < 0) {
 480                 strerror_r(errno, err_msg, sizeof(err_msg));
 481                 ERR("socketpair(): %s", err_msg);
 482                 *sock = -1;
 483                 return NULL;
 484         }
 485
 486         conn = g_malloc0(sizeof(l2cap_le_conn_info_t));
 487         g_strlcpy(conn->remote_addr, addr, BT_HAL_ADDRESS_STRING_SIZE);
 488         conn->hal_fd = fds[0];
 489         conn->stack_fd = -1;
 490         *sock = fds[1];
 491
 492         INFO("hal_fd: %d, sock: %d", conn->hal_fd, *sock);
 493
 494         return conn;
 495 }
 496
 497 int _bt_hal_dbus_handler_l2cap_le_connect(unsigned char *addr, int psm, int *sock)
 498 {
 499         int ret;
 500         l2cap_le_cb_data_t *cb_data;
 501         l2cap_le_conn_info_t *conn;
 502         char remote_addr[BT_HAL_ADDRESS_STRING_SIZE];
 503         bt_hal_l2cap_le_profile_info_t profile_info;
 504
 505         if (!addr) {
 506                 ERR("remote_addr is NULL");
 507                 return BT_STATUS_PARM_INVALID;
 508         }
 509
 510         if (!sock) {
 511                 ERR("sock is NULL");
 512                 return BT_STATUS_PARM_INVALID;
 513         }
 514
 515         cb_data = __get_l2cap_le_cb_data(psm);
 516         if (!cb_data) {
 517                 ERR("cb_Data is NULL");
 518                 return BT_STATUS_FAIL;
 519         }
 520
 521         _bt_hal_convert_addr_type_to_string(remote_addr, addr);
 522         INFO("Connecting to %s, psm %d", remote_addr, psm);
 523         conn = __l2cap_le_create_conn_info(remote_addr, sock);
 524         if (!conn) {
 525                 INFO("connection is NULL");
 526                 __bt_free_cb_data(cb_data);
 527                 return BT_STATUS_FAIL;
 528         }
 529
 530         cb_data->conn_info = conn;
 531
 532         memset(&profile_info, 0x00, sizeof(bt_hal_l2cap_le_profile_info_t));
 533
 534         profile_info.authentication = FALSE;
 535         profile_info.authorization = FALSE;
 536         profile_info.obj_path = cb_data->obj_path;
 537         profile_info.psm = psm;
 538
 539         INFO("l2cap_le connect psm: %d", profile_info.psm);
 540
 541         ret =  _bt_hal_connect_l2cap_le(remote_addr, &profile_info,
 542                                         __bt_connect_response_cb, cb_data);
 543
 544         if (ret != BT_STATUS_SUCCESS) {
 545                 ERR("Error returned while connecting l2cap_le");
 546                 __bt_free_conn(conn);
 547                 __bt_free_cb_data(cb_data);
 548                 return BT_STATUS_FAIL;
 549         }
 550
 551         INFO("Adding callback information to l2cap_le_clients");
 552         l2cap_le_clients = g_slist_append(l2cap_le_clients, cb_data);
 553
 554         return BT_STATUS_SUCCESS;
 555 }
 556
 557 /*************************** L2CAP_LE Server Implementation ***************************/
 558 static l2cap_le_server_data_t *__find_l2cap_le_server_info_from_path(const gchar *path)
 559 {
 560         GSList *l;
 561
 562         for (l = l2cap_le_servers; l != NULL; l = l->next) {
 563                 l2cap_le_server_data_t *info = l->data;
 564
 565                 if (g_strcmp0(info->obj_path, path) == 0)
 566                         return info;
 567         }
 568
 569         return NULL;
 570 }
 571
 572 static l2cap_le_server_data_t *__find_l2cap_le_server_info_from_psm(int psm)
 573 {
 574         GSList *l;
 575
 576         for (l = l2cap_le_servers; l != NULL; l = l->next) {
 577                  l2cap_le_server_data_t *info = l->data;
 578
 579                 if (info->psm == psm)
 580                         return info;
 581         }
 582
 583         return NULL;
 584 }
 585
 586
 587 static int __new_server_connection(const char *path, int fd, bt_bdaddr_t *addr)
 588 {
 589         int ret;
 590         l2cap_le_server_data_t *info;
 591         struct hal_ev_sock_connect ev;
 592
 593         INFO("%s %d", path, fd);
 594
 595         if (0 > fd) {
 596                 ERR("Invalid socket fd received");
 597                 return -1;
 598         }
 599
 600         info = __find_l2cap_le_server_info_from_path(path);
 601         if (info == NULL) {
 602                 ERR("l2cap_le server info not found");
 603                 return -1;
 604         }
 605
 606         /* Send l2cap_le client connected event */
 607         memset(&ev, 0, sizeof(ev));
 608         ev.size = sizeof(ev);
 609         memcpy(ev.bdaddr, addr->address, 6);
 610         ev.status = BT_STATUS_SUCCESS;
 611         ret = _bt_hal_send_sock_fd(info->server_fd, (void *)&ev, sizeof(ev), fd);
 612         if (ret < 0) {
 613                 ERR("Error sending connect event");
 614                 close(fd);
 615                 return -1;
 616         }
 617
 618         /* Remove local reference to client socket fd */
 619         close(fd);
 620         return 0;
 621 }
 622
 623 static void __free_l2cap_le_server_data(l2cap_le_server_data_t *data)
 624 {
 625         DBG("+");
 626
 627         if (!data) {
 628                 ERR("server data is NULL");
 629                 return;
 630         }
 631
 632         if (data->id >= 0)
 633                 __l2cap_le_delete_id(data->id);
 634
 635         if (data->object_id > 0)
 636                 _bt_hal_unregister_l2cap_le_gdbus_object(data->object_id);
 637
 638         if (data->obj_path) {
 639                 INFO("Remove socket");
 640                 _bt_hal_remove_l2cap_le_socket(data->obj_path);
 641         }
 642
 643         if (0 < data->server_fd)
 644                 close(data->server_fd);
 645
 646         if (data->server_watch > 0) {
 647                 g_source_remove(data->server_watch);
 648                 data->server_watch = 0;
 649         }
 650
 651         g_free(data->obj_path);
 652         g_free(data);
 653
 654         DBG("-");
 655 }
 656
 657 static void __remove_l2cap_le_server(l2cap_le_server_data_t *data)
 658 {
 659         DBG("+");
 660
 661         l2cap_le_servers = g_slist_remove(l2cap_le_servers, data);
 662         __free_l2cap_le_server_data(data);
 663
 664         DBG("-");
 665 }
 666
 667 static int __listen_l2cap_le_server(l2cap_le_server_data_t *server_data)
 668 {
 669         int ret;
 670         uint32_t psm;
 671         bt_hal_l2cap_le_profile_info_t profile_info;
 672
 673         INFO("+");
 674
 675         memset(&profile_info, 0x00, sizeof(bt_hal_l2cap_le_profile_info_t));
 676
 677         profile_info.authentication = FALSE;
 678         profile_info.authorization = FALSE;
 679         profile_info.obj_path = server_data->obj_path;
 680         profile_info.psm = server_data->psm;
 681
 682         INFO("Listen psm: %d", profile_info.psm);
 683
 684         ret = _bt_hal_listen_l2cap_le(&profile_info);
 685         if (ret < 0) {
 686                 ERR("Error: Listen L2CAP_LE server failed");
 687                 return BT_STATUS_FAIL;
 688         }
 689
 690         if (server_data->psm == L2CAP_LE_DEFAULT_CHANNEL) {
 691                 ret = _bt_hal_get_psm_l2cap_le(&profile_info, &psm);
 692                 if (ret < 0) {
 693                         ERR("Error: get_psm failed");
 694                         return BT_STATUS_FAIL;
 695                 }
 696                 INFO("l2cap_le server, listening to psm %d", psm);
 697                 server_data->psm = (int)psm;
 698         }
 699
 700         INFO("-");
 701         return BT_STATUS_SUCCESS;
 702 }
 703
 704 static l2cap_le_server_data_t *__create_l2cap_le_server(int psm, int *sock)
 705 {
 706         int id;
 707         int ret;
 708         int object_id;
 709         char *path;
 710         int fds[2] = {-1, -1};
 711         l2cap_le_server_data_t *data;
 712         char err_msg[256] = {0, };
 713
 714         INFO("+");
 715
 716         data = __find_l2cap_le_server_info_from_psm(psm);
 717         if (data) {
 718                 ERR("L2CAP_LE Server exists with psm: %d", psm);
 719                 return NULL;
 720         }
 721
 722         id = __l2cap_le_assign_id();
 723         if (id < 0) {
 724                 ERR("__l2cap_le_assign_id failed");
 725                 return NULL;
 726         }
 727
 728         path = g_strdup_printf("/org/socket/server/%d/%d", getpid(), id);
 729         object_id = _bt_hal_register_new_l2cap_le_gdbus_object(path, __new_server_connection);
 730         if (object_id < 0) {
 731                 ERR("_bt_hal_register_new_l2cap_le_gdbus_object failed");
 732                 g_free(path);
 733                 __l2cap_le_delete_id(id);
 734                 return NULL;
 735         }
 736
 737         data = g_malloc0(sizeof(l2cap_le_server_data_t));
 738         data->obj_path = path;
 739         data->object_id = object_id;
 740         data->id = id;
 741         data->psm = psm;
 742
 743         ret = __listen_l2cap_le_server(data);
 744         if (ret != BT_STATUS_SUCCESS) {
 745                 ERR("Error returned while registering service");
 746                 __free_l2cap_le_server_data(data);
 747                 return NULL;
 748         }
 749
 750         if (socketpair(AF_UNIX, SOCK_STREAM, 0, fds) < 0) {
 751                 strerror_r(errno, err_msg, sizeof(err_msg));
 752                 ERR("socketpair(): %s", err_msg);
 753                 __free_l2cap_le_server_data(data);
 754                 *sock = -1;
 755                 return NULL;
 756         }
 757
 758         data->server_fd = fds[0];
 759         *sock = fds[1];
 760
 761         INFO("server_fd: %d, sock: %d", data->server_fd, *sock);
 762         return data;
 763 }
 764
 765 static gboolean __server_event_cb(GIOChannel *io, GIOCondition cond, gpointer data)
 766 {
 767         gsize len;
 768         l2cap_le_server_data_t *info = data;
 769         unsigned char buff[BT_HAL_L2CAP_LE_MAX_BUFFER_SIZE];
 770         GError *err = NULL;
 771
 772         DBG("+");
 773
 774         if (cond & G_IO_HUP) {
 775                 ERR("Socket %d hang up", info->server_fd);
 776                 goto fail;
 777         }
 778
 779         if (cond & (G_IO_ERR | G_IO_NVAL)) {
 780                 ERR("Socket %d error", info->server_fd);
 781                 goto fail;
 782         }
 783
 784         /* Read data from application */
 785         if (g_io_channel_read_chars(io, (gchar *)buff,
 786                         BT_HAL_L2CAP_LE_MAX_BUFFER_SIZE,
 787                         &len, &err) == G_IO_STATUS_ERROR) {
 788                 if (err)
 789                         ERR("IO Channel read error: %s", err->message);
 790                 else
 791                         ERR("IO Channel read error client");
 792                 goto fail;
 793         }
 794
 795         INFO("len: %zu", len);
 796         if (0 == len) {
 797                 ERR("Other end of socket is closed");
 798                 goto fail;
 799         }
 800
 801         DBG("-");
 802         return TRUE;
 803 fail:
 804         __remove_l2cap_le_server(info);
 805         return FALSE;
 806 }
 807
 808 int _bt_hal_dbus_handler_l2cap_le_listen(int psm, int *sock)
 809 {
 810         int new_psm = 0;
 811         l2cap_le_server_data_t *server_data;
 812         GIOCondition cond;
 813         GIOChannel *io;
 814
 815         if (!sock) {
 816                 ERR("sock is NULL");
 817                 return BT_STATUS_PARM_INVALID;
 818         }
 819
 820         if (psm < 0 || psm > L2CAP_LE_PSM_MAX) {
 821                 ERR("Invalid PSM %d", psm);
 822                 return BT_STATUS_PARM_INVALID;
 823         }
 824
 825         server_data = __create_l2cap_le_server(psm, sock);
 826         if (!server_data)
 827                 return BT_STATUS_BUSY;
 828
 829         new_psm = server_data->psm;
 830         INFO("sending l2cap_le psm %d", new_psm);
 831
 832         if (write(server_data->server_fd, &new_psm, sizeof(new_psm)) != sizeof(new_psm)) {
 833                 ERR("Error sending l2cap_le psm");
 834                 __free_l2cap_le_server_data(server_data);
 835                 *sock = -1;
 836                 return BT_STATUS_FAIL;
 837         }
 838
 839         /* L2CAP_LE server: Handle events from Application */
 840         cond = G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL;
 841         io = g_io_channel_unix_new(server_data->server_fd);
 842         server_data->server_watch = g_io_add_watch(io, cond, __server_event_cb, server_data);
 843         g_io_channel_set_flags(io, G_IO_FLAG_NONBLOCK, NULL);
 844         g_io_channel_set_encoding(io, NULL, NULL);
 845         g_io_channel_unref(io);
 846
 847         INFO("Adding server information to l2cap_le_servers list");
 848         l2cap_le_servers = g_slist_append(l2cap_le_servers, server_data);
 849
 850         return BT_STATUS_SUCCESS;
 851 }
 852
 853 gboolean _is_l2cap_le_server_uuid(const char *uuid)
 854 {
 855         int psm;
 856         const char *value;
 857         DBG("+");
 858
 859         if (!uuid ||
 860                 strncmp(uuid, L2CAP_LE_UUID_SUBSTR, strlen(L2CAP_LE_UUID_SUBSTR)))
 861                 return FALSE;
 862
 863         value = uuid + strlen(L2CAP_LE_UUID_SUBSTR);
 864         psm = atoi(value);
 865
 866         if (NULL == __find_l2cap_le_server_info_from_psm(psm))
 867                 return FALSE;
 868
 869         DBG("-");
 870         return TRUE;
 871 }