diff --git a/router/peripheral-qrtr.c b/router/peripheral-qrtr.c index 3ce06cb..b23b9a3 100644 --- a/router/peripheral-qrtr.c +++ b/router/peripheral-qrtr.c @@ -98,6 +98,10 @@ static int qrtr_cntl_recv(int fd, void *data) } return diag_cntl_recv(perif, pkt.data, pkt.data_len); + case QRTR_TYPE_BYE: + watch_remove_writeq(perif->cntl_fd); + perif->cntl_open = false; + break; default: fprintf(stderr, "Unhandled DIAG CNTL message from %d:%d (%d)\n", pkt.node, pkt.port, pkt.type); @@ -177,6 +181,9 @@ static int qrtr_cmd_recv(int fd, void *data) err(1, "failed to connect to %d:%d", cmdsq.sq_node, cmdsq.sq_port); watch_add_writeq(perif->cmd_fd, &perif->cmdq); break; + case QRTR_TYPE_DEL_SERVER: + watch_remove_writeq(perif->cmd_fd); + break; default: fprintf(stderr, "Unhandled DIAG CMD message from %d:%d (%d)\n", pkt.node, pkt.port, pkt.type); @@ -238,6 +245,10 @@ static int qrtr_data_recv(int fd, void *data) } dm_broadcast(frame->payload, frame->length); break; + case QRTR_TYPE_BYE: + watch_remove_writeq(perif->data_fd); + perif->data_open = false; + break; default: fprintf(stderr, "Unhandled DIAG DATA message from %d:%d (%d)\n", pkt.node, pkt.port, pkt.type); diff --git a/router/watch.c b/router/watch.c index 9c101d3..6179818 100644 --- a/router/watch.c +++ b/router/watch.c @@ -195,6 +195,21 @@ void watch_remove_fd(int fd) } } +void watch_remove_writeq(int fd) +{ + struct list_head *item; + struct list_head *next; + struct watch *w; + + list_for_each_safe(item, next, &aio_watches) { + w = container_of(item, struct watch, node); + if (w->fd == fd) { + list_del(&w->node); + free(w); + } + } +} + int watch_add_quit(int (*cb)(int, void*), void *data) { struct watch *w; diff --git a/router/watch.h b/router/watch.h index 67ec75d..253c691 100644 --- a/router/watch.h +++ b/router/watch.h @@ -41,6 +41,7 @@ int watch_add_readq(int fd, struct list_head *queue, int (*cb)(struct mbuf *mbuf, void *data), void *data); int watch_add_writeq(int fd, struct list_head *queue); void watch_remove_fd(int fd); +void watch_remove_writeq(int fd); int watch_add_quit(int (*cb)(int, void*), void *data); int watch_add_timer(void (*cb)(void *), void *data, unsigned int interval, bool repeat);