From: Steve Schrock <steve.schrock@getcruise.com>
To: ofono@lists.linux.dev
Cc: Steve Schrock <steve.schrock@getcruise.com>
Subject: [PATCH v3 4/4] qmi: Enable QRTR service writes and reads
Date: Fri, 1 Mar 2024 16:18:48 -0600 [thread overview]
Message-ID: <20240301221851.1445586-4-steve.schrock@getcruise.com> (raw)
In-Reply-To: <20240301221851.1445586-1-steve.schrock@getcruise.com>
For simplicity the QRTR write method simply skips the QMUX
header. In the future it might make sense to add another ops
function that would allocate a larger buffer and populate
the QMUX header in the QMUX implementation.
---
drivers/qmimodem/qmi.c | 118 +++++++++++++++++++++++++++++++++--------
1 file changed, 97 insertions(+), 21 deletions(-)
diff --git a/drivers/qmimodem/qmi.c b/drivers/qmimodem/qmi.c
index c65860ce..8e849b75 100644
--- a/drivers/qmimodem/qmi.c
+++ b/drivers/qmimodem/qmi.c
@@ -62,7 +62,7 @@ struct qmi_service_info {
struct qmi_request {
uint16_t tid;
unsigned int group_id; /* Always 0 for control */
- uint8_t client;
+ uint8_t client; /* Always 0 for control and qrtr */
struct qmi_service_info info; /* Not used for control requests */
qmi_message_func_t callback;
void *user_data;
@@ -216,7 +216,7 @@ static void __qmi_service_appeared(struct qmi_device *device,
l_memdup(info, sizeof(struct qmi_service_info)));
}
-static struct qmi_request *__request_alloc(uint8_t service,
+static struct qmi_request *__request_alloc(uint32_t service_type,
uint8_t client, uint16_t message,
const void *data,
uint16_t length, qmi_message_func_t func,
@@ -228,7 +228,7 @@ static struct qmi_request *__request_alloc(uint8_t service,
uint16_t hdrlen = QMI_MUX_HDR_SIZE;
uint16_t msglen;
- if (service == QMI_SERVICE_CONTROL)
+ if (service_type == QMI_SERVICE_CONTROL)
hdrlen += QMI_CONTROL_HDR_SIZE;
else
hdrlen += QMI_SERVICE_HDR_SIZE;
@@ -244,7 +244,7 @@ static struct qmi_request *__request_alloc(uint8_t service,
hdr->frame = 0x01;
hdr->length = L_CPU_TO_LE16(req->len - 1);
hdr->flags = 0x00;
- hdr->service = service;
+ hdr->service = service_type; /* qmux service types are 8 bits */
hdr->client = client;
msg = (struct qmi_message_hdr *) &req->data[hdrlen];
@@ -654,6 +654,26 @@ static void __qmux_debug_msg(const char dir, const void *buf, size_t len,
L_LE16_TO_CPU(hdr->length), function, user_data);
}
+static void __qrtr_debug_msg(const char dir, const void *buf, size_t len,
+ uint16_t service_type,
+ qmi_debug_func_t function, void *user_data)
+{
+ const struct qmi_service_hdr *srv;
+ const struct qmi_message_hdr *msg;
+ uint16_t tid;
+
+ if (!len)
+ return;
+
+ srv = buf;
+ msg = buf + QMI_SERVICE_HDR_SIZE;
+
+ tid = L_LE16_TO_CPU(srv->transaction);
+
+ __debug_msg(dir, msg, service_type, srv->type, tid, 0, len,
+ function, user_data);
+}
+
static void __debug_device(struct qmi_device *device,
const char *format, ...)
{
@@ -762,7 +782,7 @@ static unsigned int service_list_create_hash(uint16_t service_type,
}
static void handle_indication(struct qmi_device *device,
- uint8_t service_type, uint8_t client_id,
+ uint32_t service_type, uint8_t client_id,
uint16_t message, uint16_t length, const void *data)
{
struct qmi_service *service;
@@ -795,7 +815,7 @@ static void handle_indication(struct qmi_device *device,
}
static void __rx_message(struct qmi_device *device,
- uint8_t service_type, uint8_t client_id,
+ uint32_t service_type, uint8_t client_id,
const void *buf)
{
const struct qmi_service_hdr *service = buf;
@@ -1935,6 +1955,43 @@ struct qmi_device_qrtr {
struct l_idle *shutdown_idle;
};
+static int qmi_device_qrtr_write(struct qmi_device *device,
+ struct qmi_request *req)
+{
+ struct sockaddr_qrtr addr;
+ uint8_t *data;
+ uint16_t len;
+ ssize_t bytes_written;
+ int fd = l_io_get_fd(device->io);
+
+ /* Skip the QMUX header */
+ data = req->data + QMI_MUX_HDR_SIZE;
+ len = req->len - QMI_MUX_HDR_SIZE;
+
+ memset(&addr, 0, sizeof(addr)); /* Ensures internal padding is 0 */
+ addr.sq_family = AF_QIPCRTR;
+ addr.sq_node = req->info.qrtr_node;
+ addr.sq_port = req->info.qrtr_port;
+
+ bytes_written = sendto(fd, data, len, 0, (struct sockaddr *) &addr,
+ sizeof(addr));
+ if (bytes_written < 0) {
+ DBG("Failure sending data: %s", strerror(errno));
+ return -errno;
+ }
+
+ l_util_hexdump(false, data, bytes_written,
+ device->debug_func, device->debug_data);
+
+ __qrtr_debug_msg(' ', data, bytes_written,
+ req->info.service_type, device->debug_func,
+ device->debug_data);
+
+ l_queue_push_tail(device->service_queue, req);
+
+ return 0;
+}
+
static void qrtr_debug_ctrl_request(const struct qrtr_ctrl_pkt *packet,
qmi_debug_func_t function,
void *user_data)
@@ -1957,10 +2014,10 @@ static void qrtr_debug_ctrl_request(const struct qrtr_ctrl_pkt *packet,
function(strbuf, user_data);
}
-static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
- const struct qrtr_ctrl_pkt *packet)
+static void qrtr_received_control_packet(struct qmi_device *device,
+ unsigned char *buf, size_t len)
{
- struct qmi_device *device = &qrtr->super;
+ struct qrtr_ctrl_pkt *packet = (struct qrtr_ctrl_pkt *) buf;
uint32_t cmd;
uint32_t type;
uint32_t instance;
@@ -1968,6 +2025,11 @@ static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
uint32_t node;
uint32_t port;
+ if (len < sizeof(*packet)) {
+ DBG("qrtr packet is too small");
+ return;
+ }
+
qrtr_debug_ctrl_request(packet, device->debug_func,
device->debug_data);
@@ -2013,22 +2075,32 @@ static void qrtr_handle_control_packet(struct qmi_device_qrtr *qrtr,
}
}
-static void qrtr_handle_packet(struct qmi_device_qrtr *qrtr, uint32_t sending_port,
- const void *buf, ssize_t len)
+static void qrtr_received_service_message(struct qmi_device *device,
+ uint32_t port, const unsigned char *buf, size_t len)
{
- const struct qrtr_ctrl_pkt *packet = buf;
+ const struct l_queue_entry *entry;
+ uint32_t service_type = 0;
- if (sending_port != QRTR_PORT_CTRL) {
- DBG("Receive of service data is not implemented");
- return;
+ for (entry = l_queue_get_entries(device->service_infos);
+ entry; entry = entry->next) {
+ struct qmi_service_info *info = entry->data;
+
+ if (info->qrtr_port == port) {
+ service_type = info->service_type;
+ break;
+ }
}
- if ((unsigned long) len < sizeof(*packet)) {
- DBG("qrtr control packet is too small");
+ if (!service_type) {
+ DBG("Received packet from unknown service on port %d",
+ port);
return;
}
- qrtr_handle_control_packet(qrtr, packet);
+ __qrtr_debug_msg(' ', buf, len, service_type,
+ device->debug_func, device->debug_data);
+
+ __rx_message(device, service_type, 0, buf);
}
static bool qrtr_received_data(struct l_io *io, void *user_data)
@@ -2042,7 +2114,7 @@ static bool qrtr_received_data(struct l_io *io, void *user_data)
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,
+ DBG("Received %zd bytes from Node: %d Port: %d", bytes_read,
addr.sq_node, addr.sq_port);
if (bytes_read < 0)
@@ -2051,7 +2123,11 @@ static bool qrtr_received_data(struct l_io *io, void *user_data)
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);
+ if (addr.sq_port == QRTR_PORT_CTRL)
+ qrtr_received_control_packet(&qrtr->super, buf, bytes_read);
+ else
+ qrtr_received_service_message(&qrtr->super, addr.sq_port,
+ buf, bytes_read);
return true;
}
@@ -2152,7 +2228,7 @@ static void qmi_device_qrtr_destroy(struct qmi_device *device)
}
static const struct qmi_device_ops qrtr_ops = {
- .write = NULL,
+ .write = qmi_device_qrtr_write,
.discover = qmi_device_qrtr_discover,
.client_create = NULL,
.client_release = NULL,
--
2.43.2
--
*Confidentiality Note:* We care about protecting our proprietary
information, confidential material, and trade secrets. This message may
contain some or all of those things. Cruise will suffer material harm if
anyone other than the intended recipient disseminates or takes any action
based on this message. If you have received this message (including any
attachments) in error, please delete it immediately and notify the sender
promptly.
prev parent reply other threads:[~2024-03-01 22:19 UTC|newest]
Thread overview: 4+ messages / expand[flat|nested] mbox.gz Atom feed top
2024-03-01 22:18 [PATCH v3 1/4] qmi: Add an abstract group id to services and requests Steve Schrock
2024-03-01 22:18 ` [PATCH v3 2/4] qmi: Store the service info in the request Steve Schrock
2024-03-01 22:18 ` [PATCH v3 3/4] qmi: Clean up the __debug_msg function Steve Schrock
2024-03-01 22:18 ` Steve Schrock [this message]
Reply instructions:
You may reply publicly to this message via plain-text email
using any one of the following methods:
* Save the following mbox file, import it into your mail client,
and reply-to-all from there: mbox
Avoid top-posting and favor interleaved quoting:
https://en.wikipedia.org/wiki/Posting_style#Interleaved_style
* Reply using the --to, --cc, and --in-reply-to
switches of git-send-email(1):
git send-email \
--in-reply-to=20240301221851.1445586-4-steve.schrock@getcruise.com \
--to=steve.schrock@getcruise.com \
--cc=ofono@lists.linux.dev \
/path/to/YOUR_REPLY
https://kernel.org/pub/software/scm/git/docs/git-send-email.html
* If your mail client supports setting the In-Reply-To header
via mailto: links, try the mailto: link
Be sure your reply has a Subject: header at the top and a blank line
before the message body.
This is a public inbox, see mirroring instructions
for how to clone and mirror all data and code used for this inbox;
as well as URLs for read-only IMAP folder(s) and NNTP newsgroup(s).