static int diag_socket_read(void *ctxt, unsigned char *buf, int buf_len)
{
int err = 0;
int pkt_len = 0;
int read_len = 0;
int bytes_remaining = 0;
int total_recd = 0;
int loop_count = 0;
uint8_t buf_full = 0;
unsigned char *temp = NULL;
struct kvec iov = {0};
struct msghdr read_msg = {0};
struct sockaddr_msm_ipc src_addr = {0};
struct diag_socket_info *info = NULL;
unsigned long flags;
info = (struct diag_socket_info *)(ctxt);
if (!info)
return -ENODEV;
if (!buf || !ctxt || buf_len <= 0)
return -EINVAL;
temp = buf;
bytes_remaining = buf_len;
err = wait_event_interruptible(info->read_wait_q,
(info->data_ready > 0) || (!info->hdl) ||
(atomic_read(&info->diag_state) == 0));
if (err) {
diagfwd_channel_read_done(info->fwd_ctxt, buf, 0);
return -ERESTARTSYS;
}
/*
* There is no need to continue reading over peripheral in this case.
* Release the wake source hold earlier.
*/
if (atomic_read(&info->diag_state) == 0) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s closing read thread. diag state is closed\n",
info->name);
diag_ws_release();
return 0;
}
if (!info->hdl) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s closing read thread\n",
info->name);
goto fail;
}
do {
loop_count++;
iov.iov_base = temp;
iov.iov_len = bytes_remaining;
read_msg.msg_name = &src_addr;
read_msg.msg_namelen = sizeof(src_addr);
pkt_len = kernel_recvmsg(info->hdl, &read_msg, &iov, 1, 0,
MSG_PEEK);
if (pkt_len <= 0)
break;
if (pkt_len > bytes_remaining) {
buf_full = 1;
break;
}
spin_lock_irqsave(&info->lock, flags);
info->data_ready--;
spin_unlock_irqrestore(&info->lock, flags);
read_len = kernel_recvmsg(info->hdl, &read_msg, &iov, 1,
pkt_len, 0);
if (read_len <= 0)
goto fail;
if (!atomic_read(&info->opened) &&
info->port_type == PORT_TYPE_SERVER) {
/*
* This is the first packet from the client. Copy its
* address to the connection object. Consider this
* channel open for communication.
*/
memcpy(&info->remote_addr, &src_addr, sizeof(src_addr));
if (info->ins_id == INST_ID_DCI)
atomic_set(&info->opened, 1);
else
__socket_open_channel(info);
}
if (read_len < 0) {
pr_err_ratelimited("diag: In %s, error receiving data, err: %d\n",
__func__, pkt_len);
err = read_len;
goto fail;
}
temp += read_len;
total_recd += read_len;
bytes_remaining -= read_len;
} while (info->data_ready > 0);
if (buf_full || (info->type == TYPE_DATA && pkt_len))
err = queue_work(info->wq, &(info->read_work));
if (total_recd > 0) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s read total bytes: %d\n",
info->name, total_recd);
err = diagfwd_channel_read_done(info->fwd_ctxt,
buf, total_recd);
if (err)
goto fail;
} else {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s error in read, err: %d\n",
info->name, total_recd);
goto fail;
}
diag_socket_queue_read(info);
return 0;
fail:
diagfwd_channel_read_done(info->fwd_ctxt, buf, 0);
return -EIO;
}
static int diag_smd_read(void *ctxt, unsigned char *buf, int buf_len)
{
int pkt_len = 0;
int err = 0;
int total_recd_partial = 0;
int total_recd = 0;
uint8_t buf_full = 0;
unsigned char *temp_buf = NULL;
uint32_t read_len = 0;
struct diag_smd_info *smd_info = NULL;
if (!ctxt || !buf || buf_len <= 0)
return -EIO;
smd_info = (struct diag_smd_info *)ctxt;
if (!smd_info->hdl || !smd_info->inited ||
!atomic_read(&smd_info->opened))
return -EIO;
/*
* Always try to read the data if notification is received from smd
* In case if packet size is 0 release the wake source hold earlier
*/
err = wait_event_interruptible(smd_info->read_wait_q,
(smd_info->hdl != NULL) &&
(atomic_read(&smd_info->opened) == 1));
if (err) {
diagfwd_channel_read_done(smd_info->fwd_ctxt, buf, 0);
return -ERESTARTSYS;
}
/*
* In this case don't reset the buffers as there is no need to further
* read over peripherals. Also release the wake source hold earlier.
*/
if (atomic_read(&smd_info->diag_state) == 0) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s closing read thread. diag state is closed\n",
smd_info->name);
diag_ws_release();
return 0;
}
if (!smd_info->hdl || !atomic_read(&smd_info->opened)) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s stopping read, hdl: %p, opened: %d\n",
smd_info->name, smd_info->hdl,
atomic_read(&smd_info->opened));
goto fail_return;
}
do {
total_recd_partial = 0;
temp_buf = buf + total_recd;
pkt_len = smd_cur_packet_size(smd_info->hdl);
if (pkt_len <= 0)
break;
if (total_recd + pkt_len > buf_len) {
buf_full = 1;
break;
}
while (total_recd_partial < pkt_len) {
read_len = smd_read_avail(smd_info->hdl);
if (!read_len) {
wait_event_interruptible(smd_info->read_wait_q,
((atomic_read(&smd_info->opened)) &&
smd_read_avail(smd_info->hdl)));
if (!smd_info->hdl ||
!atomic_read(&smd_info->opened)) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s exiting from wait",
smd_info->name);
goto fail_return;
}
}
if (pkt_len < read_len)
goto fail_return;
smd_read(smd_info->hdl, temp_buf, read_len);
total_recd_partial += read_len;
total_recd += read_len;
temp_buf += read_len;
}
} while (pkt_len > 0);
if ((smd_info->type == TYPE_DATA && pkt_len) || buf_full)
err = queue_work(smd_info->wq, &(smd_info->read_work));
if (total_recd > 0) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s read total bytes: %d\n",
smd_info->name, total_recd);
diagfwd_channel_read_done(smd_info->fwd_ctxt, buf, total_recd);
} else {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s error in read, err: %d\n",
smd_info->name, total_recd);
goto fail_return;
}
return 0;
fail_return:
diagfwd_channel_read_done(smd_info->fwd_ctxt, buf, 0);
return -EINVAL;
}
static int diag_smd_read(void *ctxt, unsigned char *buf, int buf_len)
{
int pkt_len = 0;
int err = 0;
int total_recd_partial = 0;
int total_recd = 0;
uint8_t buf_full = 0;
unsigned char *temp_buf = NULL;
uint32_t read_len = 0;
struct diag_smd_info *smd_info = NULL;
if (!ctxt || !buf || buf_len <= 0)
return -EIO;
smd_info = (struct diag_smd_info *)ctxt;
if (!smd_info->hdl || !smd_info->inited ||
!atomic_read(&smd_info->opened))
return -EIO;
wait_event(smd_info->read_wait_q, (smd_info->hdl != NULL) &&
(atomic_read(&smd_info->opened) == 1));
if (atomic_read(&smd_info->diag_state) == 0) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s closing read thread. diag state is closed\n",
smd_info->name);
diag_ws_release();
return 0;
}
if (!smd_info->hdl || !atomic_read(&smd_info->opened)) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s stopping read, hdl: %p, opened: %d\n",
smd_info->name, smd_info->hdl,
atomic_read(&smd_info->opened));
goto fail_return;
}
do {
total_recd_partial = 0;
temp_buf = buf + total_recd;
pkt_len = smd_cur_packet_size(smd_info->hdl);
if (pkt_len <= 0)
break;
if (total_recd + pkt_len > buf_len) {
buf_full = 1;
break;
}
while (total_recd_partial < pkt_len) {
read_len = smd_read_avail(smd_info->hdl);
if (!read_len) {
wait_event(smd_info->read_wait_q,
((atomic_read(&smd_info->opened)) &&
smd_read_avail(smd_info->hdl)));
if (!smd_info->hdl ||
!atomic_read(&smd_info->opened)) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s exiting from wait",
smd_info->name);
goto fail_return;
}
}
if (pkt_len < read_len)
goto fail_return;
smd_read(smd_info->hdl, temp_buf, read_len);
total_recd_partial += read_len;
total_recd += read_len;
temp_buf += read_len;
}
} while (pkt_len > 0);
if ((smd_info->type == TYPE_DATA && pkt_len) || buf_full)
err = queue_work(smd_info->wq, &(smd_info->read_work));
if (total_recd > 0) {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s read total bytes: %d\n",
smd_info->name, total_recd);
diagfwd_channel_read_done(smd_info->fwd_ctxt, buf, total_recd);
} else {
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s error in read, err: %d\n",
smd_info->name, total_recd);
goto fail_return;
}
return 0;
fail_return:
diagfwd_channel_read_done(smd_info->fwd_ctxt, buf, 0);
return -EINVAL;
}
