16 #include <linux/soc/qcom/qmi.h>
18 static struct socket *qmi_sock_create(struct qmi_handle *qmi,
23  * @qmi:	qmi handle
32 static void qmi_recv_new_server(struct qmi_handle *qmi,  in qmi_recv_new_server()  argument
36 	struct qmi_ops *ops = &qmi->ops;  in qmi_recv_new_server()
57 	ret = ops->new_server(qmi, svc);  in qmi_recv_new_server()
61 		list_add(&svc->list_node, &qmi->lookup_results);  in qmi_recv_new_server()
66  * @qmi:	qmi handle
73 static void qmi_recv_del_server(struct qmi_handle *qmi,  in qmi_recv_del_server()  argument
76 	struct qmi_ops *ops = &qmi->ops;  in qmi_recv_del_server()
80 	list_for_each_entry_safe(svc, tmp, &qmi->lookup_results, list_node) {  in qmi_recv_del_server()
87 			ops->del_server(qmi, svc);  in qmi_recv_del_server()
96  * @qmi:	qmi handle
103 static void qmi_recv_bye(struct qmi_handle *qmi,  in qmi_recv_bye()  argument
106 	struct qmi_ops *ops = &qmi->ops;  in qmi_recv_bye()
108 	qmi_recv_del_server(qmi, node, -1);  in qmi_recv_bye()
111 		ops->bye(qmi, node);  in qmi_recv_bye()
116  * @qmi:	qmi handle
122 static void qmi_recv_del_client(struct qmi_handle *qmi,  in qmi_recv_del_client()  argument
125 	struct qmi_ops *ops = &qmi->ops;  in qmi_recv_del_client()
128 		ops->del_client(qmi, node, port);  in qmi_recv_del_client()
131 static void qmi_recv_ctrl_pkt(struct qmi_handle *qmi,  in qmi_recv_ctrl_pkt()  argument
143 		qmi_recv_bye(qmi, le32_to_cpu(pkt->client.node));  in qmi_recv_ctrl_pkt()
146 		qmi_recv_new_server(qmi,  in qmi_recv_ctrl_pkt()
153 		qmi_recv_del_server(qmi,  in qmi_recv_ctrl_pkt()
158 		qmi_recv_del_client(qmi,  in qmi_recv_ctrl_pkt()
165 static void qmi_send_new_lookup(struct qmi_handle *qmi, struct qmi_service *svc)  in qmi_send_new_lookup()  argument
178 	sq.sq_family = qmi->sq.sq_family;  in qmi_send_new_lookup()
179 	sq.sq_node = qmi->sq.sq_node;  in qmi_send_new_lookup()
185 	mutex_lock(&qmi->sock_lock);  in qmi_send_new_lookup()
186 	if (qmi->sock) {  in qmi_send_new_lookup()
187 		ret = kernel_sendmsg(qmi->sock, &msg, &iv, 1, sizeof(pkt));  in qmi_send_new_lookup()
191 	mutex_unlock(&qmi->sock_lock);  in qmi_send_new_lookup()
196  * @qmi:	qmi handle
207 int qmi_add_lookup(struct qmi_handle *qmi, unsigned int service,  in qmi_add_lookup()  argument
220 	list_add(&svc->list_node, &qmi->lookups);  in qmi_add_lookup()
222 	qmi_send_new_lookup(qmi, svc);  in qmi_add_lookup()
228 static void qmi_send_new_server(struct qmi_handle *qmi, struct qmi_service *svc)  in qmi_send_new_server()  argument
240 	pkt.server.node = cpu_to_le32(qmi->sq.sq_node);  in qmi_send_new_server()
241 	pkt.server.port = cpu_to_le32(qmi->sq.sq_port);  in qmi_send_new_server()
243 	sq.sq_family = qmi->sq.sq_family;  in qmi_send_new_server()
244 	sq.sq_node = qmi->sq.sq_node;  in qmi_send_new_server()
250 	mutex_lock(&qmi->sock_lock);  in qmi_send_new_server()
251 	if (qmi->sock) {  in qmi_send_new_server()
252 		ret = kernel_sendmsg(qmi->sock, &msg, &iv, 1, sizeof(pkt));  in qmi_send_new_server()
256 	mutex_unlock(&qmi->sock_lock);  in qmi_send_new_server()
261  * @qmi:	qmi handle
267  * and start sending messages to the client associated with @qmi.
271 int qmi_add_server(struct qmi_handle *qmi, unsigned int service,  in qmi_add_server()  argument
284 	list_add(&svc->list_node, &qmi->services);  in qmi_add_server()
286 	qmi_send_new_server(qmi, svc);  in qmi_add_server()
293  * qmi_txn_init() - allocate transaction id within the given QMI handle
294  * @qmi:	QMI handle
299  * This allocates a transaction id within the QMI handle. If @ei and @c_struct
308 int qmi_txn_init(struct qmi_handle *qmi, struct qmi_txn *txn,  in qmi_txn_init()  argument
317 	txn->qmi = qmi;  in qmi_txn_init()
321 	mutex_lock(&qmi->txn_lock);  in qmi_txn_init()
322 	ret = idr_alloc_cyclic(&qmi->txns, txn, 0, U16_MAX, GFP_KERNEL);  in qmi_txn_init()
327 	mutex_unlock(&qmi->txn_lock);  in qmi_txn_init()
346 	struct qmi_handle *qmi = txn->qmi;  in qmi_txn_wait()  local
351 	mutex_lock(&qmi->txn_lock);  in qmi_txn_wait()
353 	idr_remove(&qmi->txns, txn->id);  in qmi_txn_wait()
355 	mutex_unlock(&qmi->txn_lock);  in qmi_txn_wait()
370 	struct qmi_handle *qmi = txn->qmi;  in qmi_txn_cancel()  local
372 	mutex_lock(&qmi->txn_lock);  in qmi_txn_cancel()
374 	idr_remove(&qmi->txns, txn->id);  in qmi_txn_cancel()
376 	mutex_unlock(&qmi->txn_lock);  in qmi_txn_cancel()
382  * @qmi:	qmi handle
390 static void qmi_invoke_handler(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,  in qmi_invoke_handler()  argument
398 	if (!qmi->handlers)  in qmi_invoke_handler()
401 	for (handler = qmi->handlers; handler->fn; handler++) {  in qmi_invoke_handler()
418 		handler->fn(qmi, sq, txn, dest);  in qmi_invoke_handler()
424  * qmi_handle_net_reset() - invoked to handle ENETRESET on a QMI handle
425  * @qmi:	the QMI context
437  * Finally the QMI handle will send out registration requests for any lookups
440 static void qmi_handle_net_reset(struct qmi_handle *qmi)  in qmi_handle_net_reset()  argument
446 	sock = qmi_sock_create(qmi, &sq);  in qmi_handle_net_reset()
450 	mutex_lock(&qmi->sock_lock);  in qmi_handle_net_reset()
451 	sock_release(qmi->sock);  in qmi_handle_net_reset()
452 	qmi->sock = NULL;  in qmi_handle_net_reset()
453 	mutex_unlock(&qmi->sock_lock);  in qmi_handle_net_reset()
455 	qmi_recv_del_server(qmi, -1, -1);  in qmi_handle_net_reset()
457 	if (qmi->ops.net_reset)  in qmi_handle_net_reset()
458 		qmi->ops.net_reset(qmi);  in qmi_handle_net_reset()
460 	mutex_lock(&qmi->sock_lock);  in qmi_handle_net_reset()
461 	qmi->sock = sock;  in qmi_handle_net_reset()
462 	qmi->sq = sq;  in qmi_handle_net_reset()
463 	mutex_unlock(&qmi->sock_lock);  in qmi_handle_net_reset()
465 	list_for_each_entry(svc, &qmi->lookups, list_node)  in qmi_handle_net_reset()
466 		qmi_send_new_lookup(qmi, svc);  in qmi_handle_net_reset()
468 	list_for_each_entry(svc, &qmi->services, list_node)  in qmi_handle_net_reset()
469 		qmi_send_new_server(qmi, svc);  in qmi_handle_net_reset()
472 static void qmi_handle_message(struct qmi_handle *qmi,  in qmi_handle_message()  argument
482 		pr_err("ignoring short QMI packet\n");  in qmi_handle_message()
490 		mutex_lock(&qmi->txn_lock);  in qmi_handle_message()
491 		txn = idr_find(&qmi->txns, hdr->txn_id);  in qmi_handle_message()
495 			mutex_unlock(&qmi->txn_lock);  in qmi_handle_message()
500 		mutex_unlock(&qmi->txn_lock);  in qmi_handle_message()
510 			qmi_invoke_handler(qmi, sq, txn, buf, len);  in qmi_handle_message()
519 		qmi_invoke_handler(qmi, sq, &tmp_txn, buf, len);  in qmi_handle_message()
525 	struct qmi_handle *qmi = container_of(work, struct qmi_handle, work);  in qmi_data_ready_work()  local
526 	struct qmi_ops *ops = &qmi->ops;  in qmi_data_ready_work()
533 		iv.iov_base = qmi->recv_buf;  in qmi_data_ready_work()
534 		iv.iov_len = qmi->recv_buf_size;  in qmi_data_ready_work()
536 		mutex_lock(&qmi->sock_lock);  in qmi_data_ready_work()
537 		if (qmi->sock)  in qmi_data_ready_work()
538 			msglen = kernel_recvmsg(qmi->sock, &msg, &iv, 1,  in qmi_data_ready_work()
542 		mutex_unlock(&qmi->sock_lock);  in qmi_data_ready_work()
547 			qmi_handle_net_reset(qmi);  in qmi_data_ready_work()
549 			/* The old qmi->sock is gone, our work is done */  in qmi_data_ready_work()
554 			pr_err("qmi recvmsg failed: %zd\n", msglen);  in qmi_data_ready_work()
558 		if (sq.sq_node == qmi->sq.sq_node &&  in qmi_data_ready_work()
560 			qmi_recv_ctrl_pkt(qmi, qmi->recv_buf, msglen);  in qmi_data_ready_work()
562 			ops->msg_handler(qmi, &sq, qmi->recv_buf, msglen);  in qmi_data_ready_work()
564 			qmi_handle_message(qmi, &sq, qmi->recv_buf, msglen);  in qmi_data_ready_work()
571 	struct qmi_handle *qmi = sk->sk_user_data;  in qmi_data_ready()  local
579 	if (!qmi)  in qmi_data_ready()
582 	queue_work(qmi->wq, &qmi->work);  in qmi_data_ready()
585 static struct socket *qmi_sock_create(struct qmi_handle *qmi,  in qmi_sock_create()  argument
602 	sock->sk->sk_user_data = qmi;  in qmi_sock_create()
610  * qmi_handle_init() - initialize a QMI client handle
611  * @qmi:	QMI handle to initialize
614  * @handlers:	NULL-terminated list of QMI message handlers
616  * This initializes the QMI client handle to allow sending and receiving QMI
621 int qmi_handle_init(struct qmi_handle *qmi, size_t recv_buf_size,  in qmi_handle_init()  argument
627 	mutex_init(&qmi->txn_lock);  in qmi_handle_init()
628 	mutex_init(&qmi->sock_lock);  in qmi_handle_init()
630 	idr_init(&qmi->txns);  in qmi_handle_init()
632 	INIT_LIST_HEAD(&qmi->lookups);  in qmi_handle_init()
633 	INIT_LIST_HEAD(&qmi->lookup_results);  in qmi_handle_init()
634 	INIT_LIST_HEAD(&qmi->services);  in qmi_handle_init()
636 	INIT_WORK(&qmi->work, qmi_data_ready_work);  in qmi_handle_init()
638 	qmi->handlers = handlers;  in qmi_handle_init()
640 		qmi->ops = *ops;  in qmi_handle_init()
648 	qmi->recv_buf_size = recv_buf_size;  in qmi_handle_init()
649 	qmi->recv_buf = kzalloc(recv_buf_size, GFP_KERNEL);  in qmi_handle_init()
650 	if (!qmi->recv_buf)  in qmi_handle_init()
653 	qmi->wq = alloc_ordered_workqueue("qmi_msg_handler", 0);  in qmi_handle_init()
654 	if (!qmi->wq) {  in qmi_handle_init()
659 	qmi->sock = qmi_sock_create(qmi, &qmi->sq);  in qmi_handle_init()
660 	if (IS_ERR(qmi->sock)) {  in qmi_handle_init()
661 		if (PTR_ERR(qmi->sock) == -EAFNOSUPPORT) {  in qmi_handle_init()
664 			pr_err("failed to create QMI socket\n");  in qmi_handle_init()
665 			ret = PTR_ERR(qmi->sock);  in qmi_handle_init()
673 	destroy_workqueue(qmi->wq);  in qmi_handle_init()
675 	kfree(qmi->recv_buf);  in qmi_handle_init()
682  * qmi_handle_release() - release the QMI client handle
683  * @qmi:	QMI client handle
685  * This closes the underlying socket and stops any handling of QMI messages.
687 void qmi_handle_release(struct qmi_handle *qmi)  in qmi_handle_release()  argument
689 	struct socket *sock = qmi->sock;  in qmi_handle_release()
693 	cancel_work_sync(&qmi->work);  in qmi_handle_release()
695 	qmi_recv_del_server(qmi, -1, -1);  in qmi_handle_release()
697 	mutex_lock(&qmi->sock_lock);  in qmi_handle_release()
699 	qmi->sock = NULL;  in qmi_handle_release()
700 	mutex_unlock(&qmi->sock_lock);  in qmi_handle_release()
702 	destroy_workqueue(qmi->wq);  in qmi_handle_release()
704 	idr_destroy(&qmi->txns);  in qmi_handle_release()
706 	kfree(qmi->recv_buf);  in qmi_handle_release()
709 	list_for_each_entry_safe(svc, tmp, &qmi->lookups, list_node) {  in qmi_handle_release()
715 	list_for_each_entry_safe(svc, tmp, &qmi->services, list_node) {  in qmi_handle_release()
723  * qmi_send_message() - send a QMI message
724  * @qmi:	QMI client handle
729  * @len:	max length of the QMI message
730  * @ei:		QMI message description
739 static ssize_t qmi_send_message(struct qmi_handle *qmi,  in qmi_send_message()  argument
765 	mutex_lock(&qmi->sock_lock);  in qmi_send_message()
766 	if (qmi->sock) {  in qmi_send_message()
767 		ret = kernel_sendmsg(qmi->sock, &msghdr, &iv, 1, len);  in qmi_send_message()
769 			pr_err("failed to send QMI message\n");  in qmi_send_message()
773 	mutex_unlock(&qmi->sock_lock);  in qmi_send_message()
781  * qmi_send_request() - send a request QMI message
782  * @qmi:	QMI client handle
786  * @len:	max length of the QMI message
787  * @ei:		QMI message description
792 ssize_t qmi_send_request(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,  in qmi_send_request()  argument
796 	return qmi_send_message(qmi, sq, txn, QMI_REQUEST, msg_id, len, ei,  in qmi_send_request()
802  * qmi_send_response() - send a response QMI message
803  * @qmi:	QMI client handle
807  * @len:	max length of the QMI message
808  * @ei:		QMI message description
813 ssize_t qmi_send_response(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,  in qmi_send_response()  argument
817 	return qmi_send_message(qmi, sq, txn, QMI_RESPONSE, msg_id, len, ei,  in qmi_send_response()
823  * qmi_send_indication() - send an indication QMI message
824  * @qmi:	QMI client handle
827  * @len:	max length of the QMI message
828  * @ei:		QMI message description
833 ssize_t qmi_send_indication(struct qmi_handle *qmi, struct sockaddr_qrtr *sq,  in qmi_send_indication()  argument
842 	ret = qmi_txn_init(qmi, &txn, NULL, NULL);  in qmi_send_indication()
846 	rval = qmi_send_message(qmi, sq, &txn, QMI_INDICATION, msg_id, len, ei,  in qmi_send_indication()