static int qmi_send(struct qmi_ctxt *ctxt, struct qmi_msg *msg)
{
unsigned char *data;
unsigned hlen;
unsigned len;
int r;
qmi_dump_msg(msg, "send");
if (msg->service == QMI_CTL) {
hlen = QMUX_HEADER - 1;
} else {
hlen = QMUX_HEADER;
}
/* QMUX length is total header + total payload - IFC selector */
len = hlen + msg->size - 1;
if (len > 0xffff)
return -1;
data = msg->tlv - hlen;
/* prepend encap and qmux header */
*data++ = 0x01; /* ifc selector */
/* qmux header */
*data++ = len;
*data++ = len >> 8;
*data++ = 0x00; /* flags: client */
*data++ = msg->service;
*data++ = msg->client_id;
/* qmi header */
*data++ = 0x00; /* flags: send */
*data++ = msg->txn_id;
if (msg->service != QMI_CTL)
*data++ = msg->txn_id >> 8;
*data++ = msg->type;
*data++ = msg->type >> 8;
*data++ = msg->size;
*data++ = msg->size >> 8;
/* len + 1 takes the interface selector into account */
r = smd_write(ctxt->ch, msg->tlv - hlen, len + 1);
if (r != len) {
return -1;
} else {
return 0;
}
}
static int qmi_send(struct qmi_ctxt *ctxt, struct qmi_msg *msg)
{
unsigned char *data;
unsigned hlen;
unsigned len;
int r;
qmi_dump_msg(msg, "send");
if (msg->service == QMI_CTL) {
hlen = QMUX_HEADER - 1;
} else {
hlen = QMUX_HEADER;
}
len = hlen + msg->size - 1;
if (len > 0xffff)
return -1;
data = msg->tlv - hlen;
*data++ = 0x01;
*data++ = len;
*data++ = len >> 8;
*data++ = 0x00;
*data++ = msg->service;
*data++ = msg->client_id;
*data++ = 0x00;
*data++ = msg->txn_id;
if (msg->service != QMI_CTL)
*data++ = msg->txn_id >> 8;
*data++ = msg->type;
*data++ = msg->type >> 8;
*data++ = msg->size;
*data++ = msg->size >> 8;
r = smd_write(ctxt->ch, msg->tlv - hlen, len + 1);
if (r != len) {
return -1;
} else {
return 0;
}
}
static void qmi_process_qmux(struct qmi_ctxt *ctxt,
unsigned char *buf, unsigned sz)
{
struct qmi_msg msg;
/* require a full header */
if (sz < 5)
return;
/* require a size that matches the buffer size */
if (sz != (buf[0] | (buf[1] << 8)))
return;
/* only messages from a service (bit7=1) are allowed */
if (buf[2] != 0x80)
return;
msg.service = buf[3];
msg.client_id = buf[4];
/* annoyingly, CTL messages have a shorter TID */
if (buf[3] == 0) {
if (sz < 7)
return;
msg.txn_id = buf[6];
buf += 7;
sz -= 7;
} else {
if (sz < 8)
return;
msg.txn_id = buf[6] | (buf[7] << 8);
buf += 8;
sz -= 8;
}
/* no type and size!? */
if (sz < 4)
return;
sz -= 4;
msg.type = buf[0] | (buf[1] << 8);
msg.size = buf[2] | (buf[3] << 8);
msg.tlv = buf + 4;
if (sz != msg.size)
return;
qmi_dump_msg(&msg, "recv");
mutex_lock(&ctxt->lock);
switch (msg.service) {
case QMI_CTL:
qmi_process_ctl_msg(ctxt, &msg);
break;
case QMI_WDS:
qmi_process_wds_msg(ctxt, &msg);
break;
default:
printk(KERN_ERR "qmi: msg from unknown svc 0x%02x\n",
msg.service);
break;
}
mutex_unlock(&ctxt->lock);
wake_up(&qmi_wait_queue);
}
static void qmi_process_qmux(struct qmi_ctxt *ctxt,
unsigned char *buf, unsigned sz)
{
struct qmi_msg msg;
if (sz < 5)
return;
if (sz != (buf[0] | (buf[1] << 8)))
return;
if (buf[2] != 0x80)
return;
msg.service = buf[3];
msg.client_id = buf[4];
if (buf[3] == 0) {
if (sz < 7)
return;
msg.txn_id = buf[6];
buf += 7;
sz -= 7;
} else {
if (sz < 8)
return;
msg.txn_id = buf[6] | (buf[7] << 8);
buf += 8;
sz -= 8;
}
if (sz < 4)
return;
sz -= 4;
msg.type = buf[0] | (buf[1] << 8);
msg.size = buf[2] | (buf[3] << 8);
msg.tlv = buf + 4;
if (sz != msg.size)
return;
qmi_dump_msg(&msg, "recv");
mutex_lock(&ctxt->lock);
switch (msg.service) {
case QMI_CTL:
qmi_process_ctl_msg(ctxt, &msg);
break;
case QMI_WDS:
qmi_process_wds_msg(ctxt, &msg);
break;
default:
printk(KERN_ERR "qmi: msg from unknown svc 0x%02x\n",
msg.service);
break;
}
mutex_unlock(&ctxt->lock);
wake_up(&qmi_wait_queue);
}
