1 files changed, 111 insertions, 41 deletions

diff --git a/deps/bt/c_src/bt_linux_drv.c b/deps/bt/c_src/bt_linux_drv.c
index 9dc54b9..d4fff55 100644
--- a/deps/bt/c_src/bt_linux_drv.c
+++ b/deps/bt/c_src/bt_linux_drv.c
@@ -58,6 +58,11 @@ int set_nonblock(int fd)
 #define ERR_SHORT 1
 #define ERR_LONG  2
 
+typedef struct {
+    bt_ctx_t* ctx;  // access to subscription list
+    int pending_accepts;
+    subscription_list_t acceptors;
+} linux_listen_queue_t;
 
 
 static int mgmt_open(void)
@@ -283,10 +288,10 @@ static void ddata_put_io_error(ddata_t* data, int errnum, int type)
 static void cleanup(subscription_t* s)
 {
     DEBUGF("cleanup: %s", format_subscription(s));
-    drv_data_t* lctx = (drv_data_t*) s->handle;
-
+ 
     switch(s->type) {
     case INQUIRY: {
+	drv_data_t* lctx = (drv_data_t*) s->handle;
 	bt_poll_del(lctx->inquiry_fd);
 	close(lctx->inquiry_fd);
 
@@ -302,7 +307,9 @@ static void cleanup(subscription_t* s)
     case SDP_QUERY: break;
     case SDP: break;
     case RFCOMM: {
+	DEBUGF("cleanup: Disabling and closing descriptor %d", PTR2INT(s->handle));
 	bt_poll_del(PTR2INT(s->handle));
+	shutdown(PTR2INT(s->handle), 2); // Very likely not necessary at all
 	close(PTR2INT(s->handle));
 	break;
     }
@@ -379,6 +386,8 @@ static void rfcomm_running(struct pollfd* pfd, void* arg)
 
     if (pfd->revents & POLLIN) {  // input ready
 	DEBUGF("rfcomm_running: %d has input", PTR2INT(s->handle));
+	DEBUGF("rfcomm_running: sub %p", s);
+	DEBUGF("rfcomm_running: id %d", s->id);
 
 	bt_data_len = read(pfd->fd, bt_data, sizeof(bt_data));
 	
@@ -404,17 +413,18 @@ static void rfcomm_running(struct pollfd* pfd, void* arg)
 	ddata_send(&data, 1);
 	ddata_final(&data);
 
-	release_subscription(s);
 	bt_poll_del(pfd->fd);
 	shutdown(pfd->fd, 2);
 	close(pfd->fd);
     }
+
     if (pfd->revents & POLLOUT) {  // output ready
 	DEBUGF("rfcomm_running: %d may output", PTR2INT(s->handle));
 	// FIXME: Send additional pending data.
     }
 }
 
+
 // CALLBACK 
 static void rfcomm_connected(struct pollfd* pfd, void* arg)
 {
@@ -430,24 +440,61 @@ static void rfcomm_connected(struct pollfd* pfd, void* arg)
 
 
 
-// CALLBACK 
 static void rfcomm_accept(struct pollfd* pfd, void* arg)
 {
-    subscription_t *accept_s = (subscription_t*) arg;
-
     // We have a pending client connection
+    subscription_t *listen_s = (subscription_t*) arg;
+    linux_listen_queue_t*  lq = (linux_listen_queue_t*) listen_s->opaque;
     struct sockaddr_rc rem_addr = { 0 };	
-    int client;
+    struct sockaddr_rc loc_addr = { 0 };	
+    int client_des = 0;
     socklen_t alen = sizeof(rem_addr);
     uint8_t buf[64];
     ddata_t data;
+    subscription_link_t* link = 0;
+    subscription_t* accept_s = 0;
 
-    client = accept(pfd->fd, (struct sockaddr *)&rem_addr, &alen);
+    if (listen_s->type != RFCOMM_LISTEN) {
+	DEBUGF("RFCOMM: not a listen subscription: %d", listen_s->type);
+	exit(0);
+    }
 
-    /* send EVENT id {accept,Address,Channel} */
-	
-    bt_poll_add(client, POLLIN | POLLHUP, rfcomm_running, accept_s);
-    accept_s->handle = INT2PTR(client);
+    //
+    // Find a waiting process that is currently accepting traffic
+    //
+    if (!(link = lq->acceptors.first)) {
+	DEBUGF("RFCOMM: no accepting processes. Should not happen");
+	exit(0);
+    }
+
+    //
+    // Is this the last accepting subscription?
+    // If so. Remove the listen descriptor from the bt_poll
+    // subsystem until we have another acceptor
+    //
+    if (lq->acceptors.length == 1) {
+	DEBUGF("RFCOMM: This is the last accepting process. Will disable listen");
+	bt_poll_del(pfd->fd);
+    }
+    // Retrieve the accepting subscriber process
+    accept_s = link->s;
+    
+    // Remove from acceptors waiting for an accept on this socket.
+    unlink_subscription(link);
+
+    client_des = accept(pfd->fd, (struct sockaddr *)&rem_addr, &alen);
+
+    bt_poll_add(client_des, POLLIN | POLLHUP, rfcomm_running, accept_s);
+
+    accept_s->handle = INT2PTR(client_des);
+    accept_s->accept = 0; // We are now a regular RFCOMM connection.
+
+    getsockname(client_des, (struct sockaddr *)&loc_addr, &alen);
+
+    DEBUGF("RFCOMM: accept on %X", &loc_addr.rc_channel);
+    DEBUGF("RFCOMM: desc %d", client_des);
+    DEBUGF("RFCOMM: ptr %p", accept_s);
+    DEBUGF("RFCOMM: id %p", accept_s->id);
 
     ddata_init(&data, buf, sizeof(buf), 0);
     ddata_put_UINT32(&data, 0);
@@ -456,7 +503,7 @@ static void rfcomm_accept(struct pollfd* pfd, void* arg)
     ddata_put_tag(&data, TUPLE);
     ddata_put_atom(&data, "accept");
     ddata_put_addr(&data, &rem_addr.rc_bdaddr);
-    ddata_put_uint8(&data, PTR2INT(accept_s->handle));
+    ddata_put_uint8(&data, loc_addr.rc_channel);
     ddata_put_tag(&data, TUPLE_END);
     ddata_send(&data, 1);
     ddata_final(&data);
@@ -621,7 +668,7 @@ void bt_command(bt_ctx_t* ctx, const uint8_t* src, uint32_t src_len)
 	uint32_t sid = 0;
 	uint8_t channel = 0;
 	subscription_t* listen_sub = 0;
-//	listen_queue_t* lq = 0;
+	linux_listen_queue_t* lq = 0;
 	int listen_desc = 0;
 	struct sockaddr_rc loc_addr = { 0 };
 	int dev_id;
@@ -651,6 +698,11 @@ void bt_command(bt_ctx_t* ctx, const uint8_t* src, uint32_t src_len)
 					   cleanup)) == NULL)
 	    goto mem_error;
 
+	if ((lq = alloc_type(linux_listen_queue_t)) == NULL) {
+	    release_subscription(listen_sub);
+	    goto mem_error;
+	}
+
 	dev_id = hci_get_route(NULL);
 	hci_devinfo(dev_id, &dev_info);
 	ba2str( &dev_info.bdaddr, buf );
@@ -680,9 +732,13 @@ void bt_command(bt_ctx_t* ctx, const uint8_t* src, uint32_t src_len)
 	}
 
 	listen_sub->handle = INT2PTR(listen_desc);
-	listen_sub->opaque = INT2PTR(channel);
+	listen_sub->opaque = (void* ) lq;
 	insert_last(&ctx->list, listen_sub);
 
+	// We will not add the listen descriptor to
+	// the bt_poll subsystem until we have t least
+	// one acceptor.
+
 	ddata_put_tag(&data_out, REPLY_OK);
 	ddata_put_UINT32(&data_out, cmdid);
 	goto reply;
@@ -692,8 +748,8 @@ void bt_command(bt_ctx_t* ctx, const uint8_t* src, uint32_t src_len)
     case CMD_RFCOMM_ACCEPT: { /* id:32 listen_id:32 */
 	uint32_t sid = 0;
 	uint32_t listen_id = 0;
-//	listen_queue_t* lq = 0;
-	subscription_t* listen = 0;
+	linux_listen_queue_t* lq = 0;
+	subscription_t* listen_s = 0;
 	subscription_t* s = 0;
 
 	DEBUGF("CMD_RFCOMM_ACCEPT cmdid=%d", cmdid);
@@ -713,7 +769,7 @@ void bt_command(bt_ctx_t* ctx, const uint8_t* src, uint32_t src_len)
 	    goto badarg;
 	}
 
-	if ((listen = find_subscription(&ctx->list,
+	if ((listen_s = find_subscription(&ctx->list,
 					RFCOMM_LISTEN,listen_id))==NULL) {
 	    DEBUGF("listen subscription %d does not exists", listen_id);
 	    goto badarg;
@@ -722,15 +778,26 @@ void bt_command(bt_ctx_t* ctx, const uint8_t* src, uint32_t src_len)
 	if ((s = new_subscription(RFCOMM,sid,cmdid,NULL,cleanup)) == NULL)
 	    goto mem_error;
 
-	// s->accept = listen;  // mark that we are accepting
+	s->accept = listen_s;  // mark that we are accepting
 
-	bt_poll_add(PTR2INT(listen->handle), POLLIN, rfcomm_accept, s); 
-	s->handle = listen->opaque;
+	// Are we the first acceptor added to the listen
+	// descriptor? If so, enable it.
+	lq = (linux_listen_queue_t*) listen_s->opaque;
+	if (lq->acceptors.length == 0) {
+	    DEBUGF("First acceptor for listen descriptor %d. Add to poll", listen_id);
+	    // Call rfcomm_accept with listen subscriber 
+	    // when someone connects to us.
+	    bt_poll_add(PTR2INT(listen_s->handle), POLLIN, rfcomm_accept, listen_s); 
+	}
+
+
+	insert_last(&lq->acceptors, s);
 	insert_last(&ctx->list, s);
 
 	ddata_put_tag(&data_out, REPLY_OK);
 	ddata_put_UINT32(&data_out, cmdid);
 	ddata_send(&data_out, 1);
+	
 	goto done;
     }
 
@@ -748,34 +815,36 @@ void bt_command(bt_ctx_t* ctx, const uint8_t* src, uint32_t src_len)
 	if ((link = find_subscription_link(&ctx->list,RFCOMM,sid)) != NULL) {
 	    subscription_t* s = link->s;
 	    int sock;
+	    DEBUGF("CMD_RFCOMM_CLOSE found RFCOMM link", cmdid);
 	    s->cmdid = cmdid;
 	    sock = PTR2INT(s->handle);
 	    if (sock >= 0) {
 		DEBUGF("RFCOMM_CLOSE: channel=%d", sock);
-		bt_poll_del(sock);
-		close(sock);
-		unlink_subscription(link);
-		goto done;
+		unlink_subscription(link); // Will close and unlink s->handle
+		goto ok;
 	    }
-	    /* else if (s->accept != NULL) { */
-	    /* 	listen_queue_t* lq = (listen_queue_t*)((s->accept)->opaque); */
-	    /* 	remove_subscription(&lq->wait,RFCOMM,sid); */
-	    /* 	unlink_subscription(link); */
-	    /* 	goto ok; */
-	    /* } */
+	    else if (s->accept != NULL) { 
+		DEBUGF("RFCOMM_CLOSE: This is an acceptor with no session");
+		linux_listen_queue_t* lq = (linux_listen_queue_t*)((s->accept)->opaque); 
+		remove_subscription(&lq->acceptors,RFCOMM,sid); 
+		unlink_subscription(link); 
+		goto ok; 
+	    } 
 	}
 	else if ((link = find_subscription_link(&ctx->list,RFCOMM_LISTEN,sid)) != NULL) {
-	    /* subscription_t* listen = link->s; */
-	    /* listen_queue_t* lq = (listen_queue_t*)listen->opaque; */
-	    /* subscription_link_t* link1; */
-	    /* /\* remove all waiters *\/ */
-	    /* while((link1=lq->wait.first) != NULL) { */
-	    /* 	send_event(link1->s->id, "closed"); */
-	    /* 	unlink_subscription(link1); */
-	    /* } */
-	    /* unlink_subscription(link); */
+	    subscription_t* listen = link->s;
+	    linux_listen_queue_t* lq = (linux_listen_queue_t*)listen->opaque;
+	    subscription_link_t* link1; 
+	    DEBUGF("RFCOMM_CLOSE: This is a listen subscriuber");
+	    /* remove all waiters */
+	    while((link1=lq->acceptors.first) != NULL) { 
+	     	send_event(link1->s->id, "closed"); 
+	     	unlink_subscription(link1);
+	    }
+	    unlink_subscription(link);
 	    goto ok;
 	}
+	    DEBUGF("RFCOMM_CLOSE: Shit");
 	goto error;
     }
 
@@ -980,7 +1049,8 @@ void read_callback(struct pollfd* pfd, void* data)
     int fd = pfd->fd;
     int n;
 
-    DEBUGF("read_callback: %d", pfd->fd);
+    DEBUGF("read_callback: %d:%X", pfd->fd, pfd->revents);
+
     if (pfd->revents & POLLHUP)  {
 	DEBUGF("hangup");
 	goto closed;