@@ -33,6 +33,8 @@
#include <stdlib.h>
#include <string.h>
#include <linux/limits.h>
+#include <linux/qrtr.h>
+#include <sys/socket.h>
#include <ell/ell.h>
@@ -784,10 +786,22 @@ static void __qmi_device_discovery_complete(struct qmi_device *device,
{
if (!l_queue_remove(device->discovery_queue, d))
return;
-
- __discovery_free(d);
}
+/*
+ * Prevents re-entrancy problems by removing the entry from the discovery_queue
+ * before calling the callback.
+ */
+#define DISCOVERY_DONE(data, ...)\
+do {\
+ __qmi_device_discovery_complete(data->device, &data->super);\
+\
+ if (data->func)\
+ data->func(__VA_ARGS__);\
+\
+ __discovery_free(&data->super);\
+} while (0)
+
static void service_destroy(void *data)
{
struct qmi_service *service = data;
@@ -1344,9 +1358,8 @@ static void service_create_shared_reply(struct l_idle *idle, void *user_data)
l_idle_remove(data->idle);
data->idle = NULL;
- data->func(data->service, data->user_data);
- __qmi_device_discovery_complete(data->device, &data->super);
+ DISCOVERY_DONE(data, data->service, data->user_data);
}
static void service_create_shared_pending_reply(struct qmi_device *device,
@@ -1406,10 +1419,7 @@ static void qmux_sync_callback(uint16_t message, uint16_t length,
{
struct discover_data *data = user_data;
- if (data->func)
- data->func(data->user_data);
-
- __qmi_device_discovery_complete(data->device, &data->super);
+ DISCOVERY_DONE(data, data->user_data);
}
/* sync will release all previous clients */
@@ -1510,10 +1520,7 @@ done:
return;
}
- if (data->func)
- data->func(data->user_data);
-
- __qmi_device_discovery_complete(data->device, &data->super);
+ DISCOVERY_DONE(data, data->user_data);
}
static void qmux_discover_reply_timeout(struct l_timeout *timeout,
@@ -1531,10 +1538,7 @@ static void qmux_discover_reply_timeout(struct l_timeout *timeout,
/* remove request from queues */
req = find_control_request(qmux, data->tid);
- if (data->func)
- data->func(data->user_data);
-
- __qmi_device_discovery_complete(device, &data->super);
+ DISCOVERY_DONE(data, data->user_data);
if (req)
__request_free(req);
@@ -1620,10 +1624,7 @@ static void qmux_client_create_reply(struct l_timeout *timeout, void *user_data)
l_timeout_remove(data->timeout);
data->timeout = NULL;
- if (data->func)
- data->func(NULL, data->user_data);
-
- __qmi_device_discovery_complete(device, &data->super);
+ DISCOVERY_DONE(data, NULL, data->user_data);
if (req)
__request_free(req);
@@ -1684,10 +1685,8 @@ static void qmux_client_create_callback(uint16_t message, uint16_t length,
done:
service_create_shared_pending_reply(device, data->type, service);
- data->func(service, data->user_data);
+ DISCOVERY_DONE(data, service, data->user_data);
qmi_service_unref(service);
-
- __qmi_device_discovery_complete(data->device, &data->super);
}
static int qmi_device_qmux_client_create(struct qmi_device *device,
@@ -1874,6 +1873,250 @@ struct qmi_device *qmi_device_new_qmux(const char *device)
return &qmux->super;
}
+struct qmi_device_qrtr {
+ struct qmi_device super;
+ qmi_shutdown_func_t shutdown_func;
+ void *shutdown_user_data;
+ qmi_destroy_func_t shutdown_destroy;
+ struct l_idle *shutdown_idle;
+};
+
+static void qrtr_debug_ctrl_request(const struct qrtr_ctrl_pkt *packet,
+ qmi_debug_func_t function,
+ void *user_data)
+{
+ char strbuf[72 + 16], *str;
+ const char *type;
+
+ if (!function)
+ return;
+
+ str = strbuf;
+ str += sprintf(str, " %s",
+ __service_type_to_string(QMI_SERVICE_CONTROL));
+
+ type = "_pkt";
+
+ str += sprintf(str, "%s cmd=%d", type,
+ L_LE32_TO_CPU(packet->cmd));
+
+ function(strbuf, user_data);
+}
+
+static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
+ const struct qrtr_ctrl_pkt *packet)
+{
+ struct qmi_device *device = &qrtr->super;
+ uint32_t cmd;
+ uint32_t type;
+ uint32_t instance;
+ uint32_t version;
+ uint32_t node;
+ uint32_t port;
+
+ qrtr_debug_ctrl_request(packet, device->debug_func,
+ device->debug_data);
+
+ cmd = L_LE32_TO_CPU(packet->cmd);
+ if (cmd != QRTR_TYPE_NEW_SERVER) {
+ DBG("Unknown command: %d", cmd);
+ return;
+ }
+
+ if (!packet->server.service && !packet->server.instance &&
+ !packet->server.node && !packet->server.port) {
+ struct discover_data *data;
+
+ DBG("Initial service discovery has completed");
+
+ data = l_queue_peek_head(device->discovery_queue);
+ DISCOVERY_DONE(data, data->user_data);
+
+ return;
+ }
+
+ type = L_LE32_TO_CPU(packet->server.service);
+ version = L_LE32_TO_CPU(packet->server.instance) & 0xff;
+ instance = L_LE32_TO_CPU(packet->server.instance) >> 8;
+
+ node = L_LE32_TO_CPU(packet->server.node);
+ port = L_LE32_TO_CPU(packet->server.port);
+
+ if (cmd == QRTR_TYPE_NEW_SERVER) {
+ DBG("New server: Type: %d Version: %d Instance: %d Node: %d Port: %d",
+ type, version, instance, node, port);
+ }
+}
+
+static void qrtr_handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port,
+ const void *buf, ssize_t len)
+{
+ const struct qrtr_ctrl_pkt *packet = buf;
+
+ if (sending_port != QRTR_PORT_CTRL) {
+ DBG("Receive of service data is not implemented");
+ return;
+ }
+
+ if ((unsigned long) len < sizeof(*packet)) {
+ DBG("qrtr control packet is too small");
+ return;
+ }
+
+ qrtr_handle_control_packet(qrtr, packet);
+}
+
+static bool qrtr_received_data(struct l_io *io, void *user_data)
+{
+ struct qmi_device_qrtr *qrtr = user_data;
+ struct sockaddr_qrtr addr;
+ unsigned char buf[2048];
+ ssize_t bytes_read;
+ socklen_t addr_size;
+
+ addr_size = sizeof(addr);
+ bytes_read = recvfrom(l_io_get_fd(qrtr->super.io), buf, sizeof(buf), 0,
+ (struct sockaddr *) &addr, &addr_size);
+ DBG("Received %zd bytes from Node: %d Port: 0x%x", bytes_read,
+ addr.sq_node, addr.sq_port);
+
+ if (bytes_read < 0)
+ return true;
+
+ l_util_hexdump(true, buf, bytes_read, qrtr->super.debug_func,
+ qrtr->super.debug_data);
+
+ qrtr_handle_packet(qrtr, addr.sq_port, buf, bytes_read);
+
+ return true;
+}
+
+static void qrtr_discover_reply_timeout(struct l_timeout *timeout,
+ void *user_data)
+{
+ struct discover_data *data = user_data;
+
+ l_timeout_remove(data->timeout);
+ data->timeout = NULL;
+
+ DISCOVERY_DONE(data, data->user_data);
+}
+
+static int qmi_device_qrtr_discover(struct qmi_device *device,
+ qmi_discover_func_t func,
+ void *user_data,
+ qmi_destroy_func_t destroy)
+{
+ struct discover_data *data;
+ struct qrtr_ctrl_pkt packet;
+ struct sockaddr_qrtr addr;
+ socklen_t addr_len;
+ int rc;
+ ssize_t bytes_written;
+ int fd;
+
+ __debug_device(device, "device %p discover", device);
+
+ if (l_queue_length(device->discovery_queue) > 0)
+ return -EINPROGRESS;
+
+ data = l_new(struct discover_data, 1);
+
+ data->super.destroy = discover_data_free;
+ data->device = device;
+ data->func = func;
+ data->user_data = user_data;
+ data->destroy = destroy;
+
+ fd = l_io_get_fd(device->io);
+
+ /*
+ * The control node is configured by the system. Use getsockname to
+ * get its value.
+ */
+ addr_len = sizeof(addr);
+ rc = getsockname(fd, (struct sockaddr *) &addr, &addr_len);
+ if (rc) {
+ DBG("getsockname failed: %s", strerror(errno));
+ rc = -errno;
+ goto error;
+ }
+
+ if (addr.sq_family != AF_QIPCRTR || addr_len != sizeof(addr)) {
+ DBG("Unexpected sockaddr from getsockname. family: %d size: %d",
+ addr.sq_family, addr_len);
+ rc = -EIO;
+ goto error;
+ }
+
+ addr.sq_port = QRTR_PORT_CTRL;
+ memset(&packet, 0, sizeof(packet));
+ packet.cmd = L_CPU_TO_LE32(QRTR_TYPE_NEW_LOOKUP);
+
+ bytes_written = sendto(fd, &packet,
+ sizeof(packet), 0,
+ (struct sockaddr *) &addr, addr_len);
+ if (bytes_written < 0) {
+ DBG("Failure sending data: %s", strerror(errno));
+ rc = -errno;
+ goto error;
+ }
+
+ l_util_hexdump(false, &packet, bytes_written,
+ device->debug_func, device->debug_data);
+
+ data->timeout = l_timeout_create(5, qrtr_discover_reply_timeout,
+ data, NULL);
+
+ __qmi_device_discovery_started(device, &data->super);
+
+ return 0;
+
+error:
+ __discovery_free(&data->super);
+
+ return rc;
+}
+
+static void qmi_device_qrtr_destroy(struct qmi_device *device)
+{
+ struct qmi_device_qrtr *qrtr =
+ l_container_of(device, struct qmi_device_qrtr, super);
+
+ l_free(qrtr);
+}
+
+static const struct qmi_device_ops qrtr_ops = {
+ .write = NULL,
+ .discover = qmi_device_qrtr_discover,
+ .client_create = NULL,
+ .client_release = NULL,
+ .shutdown = NULL,
+ .destroy = qmi_device_qrtr_destroy,
+};
+
+struct qmi_device *qmi_device_new_qrtr(void)
+{
+ struct qmi_device_qrtr *qrtr;
+ int fd;
+
+ fd = socket(AF_QIPCRTR, SOCK_DGRAM, 0);
+ if (fd < 0)
+ return NULL;
+
+ qrtr = l_new(struct qmi_device_qrtr, 1);
+
+ if (qmi_device_init(&qrtr->super, fd, &qrtr_ops) < 0) {
+ close(fd);
+ l_free(qrtr);
+ return NULL;
+ }
+
+ l_io_set_read_handler(qrtr->super.io, qrtr_received_data, qrtr, NULL);
+
+ return &qrtr->super;
+}
+
struct qmi_param *qmi_param_new(void)
{
struct qmi_param *param;
@@ -101,6 +101,7 @@ bool qmi_device_set_expected_data_format(struct qmi_device *device,
enum qmi_device_expected_data_format format);
struct qmi_device *qmi_device_new_qmux(const char *device);
+struct qmi_device *qmi_device_new_qrtr(void);
struct qmi_param;