static void smd_notify(void *ctxt, unsigned event)
{
struct diag_smd_info *smd_info = NULL;
smd_info = (struct diag_smd_info *)ctxt;
if (!smd_info)
return;
switch (event) {
case SMD_EVENT_OPEN:
atomic_set(&smd_info->opened, 1);
smd_info->fifo_size = smd_write_avail(smd_info->hdl);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s channel opened\n",
smd_info->name);
queue_work(smd_info->wq, &(smd_info->open_work));
break;
case SMD_EVENT_CLOSE:
atomic_set(&smd_info->opened, 0);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s channel closed\n",
smd_info->name);
queue_work(smd_info->wq, &(smd_info->close_work));
break;
case SMD_EVENT_DATA:
diag_ws_on_notify();
queue_work(smd_info->wq, &(smd_info->read_work));
break;
}
wake_up_interruptible(&smd_info->read_wait_q);
}
static void __socket_close_channel(struct diag_socket_info *info)
{
if (!info || !info->hdl)
return;
if (!atomic_read(&info->opened))
return;
memset(&info->remote_addr, 0, sizeof(struct sockaddr_msm_ipc));
diagfwd_channel_close(info->fwd_ctxt);
atomic_set(&info->opened, 0);
/* Don't close the server. Server should always remain open */
if (info->port_type != PORT_TYPE_SERVER) {
write_lock_bh(&info->hdl->sk->sk_callback_lock);
info->hdl->sk->sk_user_data = NULL;
info->hdl->sk->sk_data_ready = NULL;
write_unlock_bh(&info->hdl->sk->sk_callback_lock);
sock_release(info->hdl);
info->hdl = NULL;
wake_up_interruptible(&info->read_wait_q);
}
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n", info->name);
return;
}
static void __diag_smd_init(struct diag_smd_info *smd_info)
{
char wq_name[DIAG_SMD_NAME_SZ + 10];
if (!smd_info)
return;
init_waitqueue_head(&smd_info->read_wait_q);
mutex_init(&smd_info->lock);
strlcpy(wq_name, "DIAG_SMD_", 10);
strlcat(wq_name, smd_info->name, sizeof(smd_info->name));
smd_info->wq = create_singlethread_workqueue(wq_name);
if (!smd_info->wq) {
pr_err("diag: In %s, unable to create workqueue for smd channel %s\n",
__func__, smd_info->name);
return;
}
INIT_WORK(&(smd_info->open_work), smd_open_work_fn);
INIT_WORK(&(smd_info->close_work), smd_close_work_fn);
INIT_WORK(&(smd_info->read_work), smd_read_work_fn);
smd_info->fifo_size = 0;
smd_info->hdl = NULL;
smd_info->fwd_ctxt = NULL;
atomic_set(&smd_info->opened, 0);
atomic_set(&smd_info->diag_state, 0);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s initialized fwd_ctxt: %p\n",
smd_info->name, smd_info->fwd_ctxt);
}
void ServiceListeners::SendFrameworkEvent(const FrameworkEvent& evt)
{
// avoid deadlocks, race conditions and other undefined behavior
// by using a local snapshot of all listeners.
// A lock shouldn't be held while calling into user code (e.g. callbacks).
FrameworkListenerMap listener_snapshot;
{
auto l = frameworkListenerMap.Lock(); US_UNUSED(l);
listener_snapshot = frameworkListenerMap.value;
}
for (auto& listeners : listener_snapshot)
{
for (auto& listener : listeners.second)
{
try
{
std::get<0>(listener.second)(evt);
}
catch (...)
{
// do not send a FrameworkEvent as that could cause a deadlock or an infinite loop.
// Instead, log to the internal logger
// @todo send this to the LogService instead when its supported.
DIAG_LOG(*coreCtx->sink) << "A Framework Listener threw an exception: " << util::GetLastExceptionStr() << "\n";
}
}
}
}
static void socket_open_client(struct diag_socket_info *info)
{
int ret = 0;
if (!info || info->port_type != PORT_TYPE_CLIENT)
return;
ret = sock_create(AF_MSM_IPC, SOCK_DGRAM, 0, &info->hdl);
if (ret < 0 || !info->hdl) {
pr_err("diag: In %s, socket not initialized for %s\n", __func__,
info->name);
return;
}
write_lock_bh(&info->hdl->sk->sk_callback_lock);
info->hdl->sk->sk_user_data = (void *)(info);
info->hdl->sk->sk_data_ready = socket_data_ready;
info->hdl->sk->sk_write_space = socket_flow_cntl;
write_unlock_bh(&info->hdl->sk->sk_callback_lock);
ret = lookup_server(info);
if (ret) {
pr_err("diag: In %s, failed to lookup server, ret: %d\n",
__func__, ret);
return;
}
__socket_open_channel(info);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n", info->name);
}
static int cntl_socket_process_msg_server(uint32_t cmd, uint32_t svc_id,
uint32_t ins_id)
{
uint8_t peripheral;
uint8_t found = 0;
struct diag_socket_info *info = NULL;
for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
info = &socket_cmd[peripheral];
if ((svc_id == info->svc_id) &&
(ins_id == info->ins_id)) {
found = 1;
break;
}
info = &socket_dci_cmd[peripheral];
if ((svc_id == info->svc_id) &&
(ins_id == info->ins_id)) {
found = 1;
break;
}
}
if (!found)
return -EIO;
switch (cmd) {
case CNTL_CMD_NEW_SERVER:
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s received new server\n",
info->name);
diagfwd_register(TRANSPORT_SOCKET, info->peripheral,
info->type, (void *)info, &socket_ops,
&info->fwd_ctxt);
queue_work(info->wq, &(info->init_work));
break;
case CNTL_CMD_REMOVE_SERVER:
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s received remove server\n",
info->name);
socket_close_channel(info);
break;
default:
return -EINVAL;
}
return 0;
}
static void socket_close_channel(struct diag_socket_info *info)
{
if (!info)
return;
__socket_close_channel(info);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n", info->name);
}
static void __diag_smd_exit(struct diag_smd_info *smd_info)
{
if (!smd_info)
return;
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s entering\n",
smd_info->name);
diagfwd_deregister(smd_info->peripheral, smd_info->type,
(void *)smd_info);
smd_info->fwd_ctxt = NULL;
smd_info->hdl = NULL;
if (smd_info->wq)
destroy_workqueue(smd_info->wq);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n",
smd_info->name);
}
static void smd_late_init(struct diag_smd_info *smd_info)
{
struct diagfwd_info *fwd_info = NULL;
if (!smd_info)
return;
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s entering\n",
smd_info->name);
diagfwd_register(TRANSPORT_SMD, smd_info->peripheral, smd_info->type,
(void *)smd_info, &smd_ops, &smd_info->fwd_ctxt);
fwd_info = smd_info->fwd_ctxt;
smd_info->inited = 1;
if (atomic_read(&smd_info->opened))
diagfwd_channel_open(smd_info->fwd_ctxt);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n",
smd_info->name);
}
static void diag_state_open_smd(void *ctxt)
{
struct diag_smd_info *smd_info = NULL;
if (!ctxt)
return;
smd_info = (struct diag_smd_info *)(ctxt);
atomic_set(&smd_info->diag_state, 1);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s setting diag state to 1", smd_info->name);
}
static void smd_close_work_fn(struct work_struct *work)
{
struct diag_smd_info *smd_info = container_of(work,
struct diag_smd_info,
close_work);
if (!smd_info->inited)
return;
diagfwd_channel_close(smd_info->fwd_ctxt);
wake_up_interruptible(&smd_info->read_wait_q);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n",
smd_info->name);
}
static void smd_open_work_fn(struct work_struct *work)
{
struct diag_smd_info *smd_info = container_of(work,
struct diag_smd_info,
open_work);
if (!smd_info->inited)
return;
diagfwd_channel_open(smd_info->fwd_ctxt);
diagfwd_late_open(smd_info->fwd_ctxt);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n",
smd_info->name);
}
void diag_smd_invalidate(void *ctxt, struct diagfwd_info *fwd_ctxt)
{
struct diag_smd_info *smd_info = NULL;
void *prev = NULL;
if (!ctxt || !fwd_ctxt)
return;
smd_info = (struct diag_smd_info *)ctxt;
prev = smd_info->fwd_ctxt;
smd_info->fwd_ctxt = fwd_ctxt;
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s prev: %p fwd_ctxt: %p\n",
smd_info->name, prev, smd_info->fwd_ctxt);
}
static void diag_state_close_smd(void *ctxt)
{
struct diag_smd_info *smd_info = NULL;
if (!ctxt)
return;
smd_info = (struct diag_smd_info *)(ctxt);
atomic_set(&smd_info->diag_state, 0);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s setting diag state to 0", smd_info->name);
wake_up_interruptible(&smd_info->read_wait_q);
flush_workqueue(smd_info->wq);
}
static void smd_late_init(struct diag_smd_info *smd_info)
{
struct diagfwd_info *fwd_info = NULL;
if (!smd_info)
return;
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s entering\n",
smd_info->name);
diagfwd_register(TRANSPORT_SMD, smd_info->peripheral, smd_info->type,
(void *)smd_info, &smd_ops, &smd_info->fwd_ctxt);
fwd_info = smd_info->fwd_ctxt;
smd_info->inited = 1;
/*
* The channel is already open by the probe call as a result of other
* peripheral. Inform the diag fwd layer that the channel is open.
*/
if (atomic_read(&smd_info->opened))
diagfwd_channel_open(smd_info->fwd_ctxt);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n",
smd_info->name);
}
static int cntl_socket_process_msg_client(uint32_t cmd, uint32_t node_id,
uint32_t port_id)
{
uint8_t peripheral;
uint8_t found = 0;
struct diag_socket_info *info = NULL;
struct msm_ipc_port_addr remote_port = {0};
for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
info = &socket_data[peripheral];
remote_port = info->remote_addr.address.addr.port_addr;
if ((remote_port.node_id == node_id) &&
(remote_port.port_id == port_id)) {
found = 1;
break;
}
info = &socket_cntl[peripheral];
remote_port = info->remote_addr.address.addr.port_addr;
if ((remote_port.node_id == node_id) &&
(remote_port.port_id == port_id)) {
found = 1;
break;
}
info = &socket_dci[peripheral];
remote_port = info->remote_addr.address.addr.port_addr;
if ((remote_port.node_id == node_id) &&
(remote_port.port_id == port_id)) {
found = 1;
break;
}
}
if (!found)
return -EIO;
switch (cmd) {
case CNTL_CMD_REMOVE_CLIENT:
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s received remove client\n",
info->name);
socket_close_channel(info);
break;
default:
return -EINVAL;
}
return 0;
}
static int diag_smd_write(void *ctxt, unsigned char *buf, int len)
{
int write_len = 0;
int retry_count = 0;
int max_retries = 3;
struct diag_smd_info *smd_info = NULL;
if (!ctxt || !buf)
return -EIO;
smd_info = (struct diag_smd_info *)ctxt;
if (!smd_info || !buf || len <= 0) {
pr_err_ratelimited("diag: In %s, invalid params, smd_info: %p, buf: %p, len: %d\n",
__func__, smd_info, buf, len);
return -EINVAL;
}
if (!smd_info->inited || !smd_info->hdl ||
!atomic_read(&smd_info->opened))
return -ENODEV;
if (len > smd_info->fifo_size)
return diag_smd_write_ext(smd_info, buf, len);
do {
mutex_lock(&smd_info->lock);
write_len = smd_write(smd_info->hdl, buf, len);
mutex_unlock(&smd_info->lock);
if (write_len == len)
break;
/*
* The channel maybe busy - the FIFO can be full. Retry after
* sometime. The value of 10000 was chosen emprically as the
* optimal value for the peripherals to read data from the SMD
* channel.
*/
usleep_range(10000, 10100);
retry_count++;
} while (retry_count < max_retries);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s wrote to channel, write_len: %d\n",
smd_info->name, write_len);
if (write_len != len)
return -ENOMEM;
return 0;
}
static void socket_flow_cntl(struct sock *sk_ptr)
{
struct diag_socket_info *info = NULL;
if (!sk_ptr)
return;
info = (struct diag_socket_info *)(sk_ptr->sk_user_data);
if (!info) {
pr_err_ratelimited("diag: In %s, invalid info\n", __func__);
return;
}
atomic_inc(&info->flow_cnt);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s flow controlled\n", info->name);
pr_debug("diag: In %s, channel %s flow controlled\n",
__func__, info->name);
}
static int lookup_server(struct diag_socket_info *info)
{
int ret = 0;
struct server_lookup_args *args = NULL;
struct sockaddr_msm_ipc *srv_addr = NULL;
if (!info)
return -EINVAL;
args = kzalloc((sizeof(struct server_lookup_args) +
sizeof(struct msm_ipc_server_info)), GFP_KERNEL);
if (!args)
return -ENOMEM;
kmemleak_not_leak(args);
args->lookup_mask = 0xFFFFFFFF;
args->port_name.service = info->svc_id;
args->port_name.instance = info->ins_id;
args->num_entries_in_array = 1;
args->num_entries_found = 0;
ret = kernel_sock_ioctl(info->hdl, IPC_ROUTER_IOCTL_LOOKUP_SERVER,
(unsigned long)args);
if (ret < 0) {
pr_err("diag: In %s, cannot find service for %s\n", __func__,
info->name);
kfree(args);
return -EFAULT;
}
srv_addr = &info->remote_addr;
srv_addr->family = AF_MSM_IPC;
srv_addr->address.addrtype = MSM_IPC_ADDR_ID;
srv_addr->address.addr.port_addr.node_id = args->srv_info[0].node_id;
srv_addr->address.addr.port_addr.port_id = args->srv_info[0].port_id;
ret = args->num_entries_found;
kfree(args);
if (ret < 1)
return -EIO;
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s found server node: %d port: %d",
info->name, srv_addr->address.addr.port_addr.node_id,
srv_addr->address.addr.port_addr.port_id);
return 0;
}
static int diag_smd_write(void *ctxt, unsigned char *buf, int len)
{
int write_len = 0;
int retry_count = 0;
int max_retries = 3;
struct diag_smd_info *smd_info = NULL;
if (!ctxt || !buf)
return -EIO;
smd_info = (struct diag_smd_info *)ctxt;
if (!smd_info || !buf || len <= 0) {
pr_err_ratelimited("diag: In %s, invalid params, smd_info: %p, buf: %p, len: %d\n",
__func__, smd_info, buf, len);
return -EINVAL;
}
if (!smd_info->inited || !smd_info->hdl ||
!atomic_read(&smd_info->opened))
return -ENODEV;
if (len > smd_info->fifo_size)
return diag_smd_write_ext(smd_info, buf, len);
do {
mutex_lock(&smd_info->lock);
write_len = smd_write(smd_info->hdl, buf, len);
mutex_unlock(&smd_info->lock);
if (write_len == len)
break;
usleep_range(10000, 10100);
retry_count++;
} while (retry_count < max_retries);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s wrote to channel, write_len: %d\n",
smd_info->name, write_len);
if (write_len != len)
return -ENOMEM;
return 0;
}
static int diag_socket_write(void *ctxt, unsigned char *buf, int len)
{
int err = 0;
int write_len = 0;
struct kvec iov = {0};
struct msghdr write_msg = {0};
struct diag_socket_info *info = NULL;
if (!ctxt || !buf || len <= 0)
return -EIO;
info = (struct diag_socket_info *)(ctxt);
if (!atomic_read(&info->opened) || !info->hdl)
return -ENODEV;
iov.iov_base = buf;
iov.iov_len = len;
write_msg.msg_name = &info->remote_addr;
write_msg.msg_namelen = sizeof(info->remote_addr);
write_msg.msg_flags |= MSG_DONTWAIT;
write_len = kernel_sendmsg(info->hdl, &write_msg, &iov, 1, len);
if (write_len < 0) {
err = write_len;
/*
* -EAGAIN means that the number of packets in flight is at
* max capactity and the peripheral hasn't read the data.
*/
if (err != -EAGAIN) {
pr_err_ratelimited("diag: In %s, error sending data, err: %d, ch: %s\n",
__func__, err, info->name);
}
} else if (write_len != len) {
err = write_len;
pr_err_ratelimited("diag: In %s, wrote partial packet to %s, len: %d, wrote: %d\n",
__func__, info->name, len, write_len);
}
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s wrote to socket, len: %d\n",
info->name, write_len);
return err;
}
ServiceReferenceBase ServiceRegistry::Get(BundlePrivate* bundle,
const std::string& clazz) const
{
auto l = this->Lock();
US_UNUSED(l);
try {
std::vector<ServiceReferenceBase> srs;
Get_unlocked(clazz, "", bundle, srs);
DIAG_LOG(*core->sink) << "get service ref " << clazz << " for bundle "
<< bundle->symbolicName << " = " << srs.size()
<< " refs";
if (!srs.empty()) {
return srs.back();
}
} catch (const std::invalid_argument&) {
}
return ServiceReferenceBase();
}
static void socket_open_server(struct diag_socket_info *info)
{
int ret = 0;
struct sockaddr_msm_ipc srv_addr = { 0 };
if (!info)
return;
ret = sock_create(AF_MSM_IPC, SOCK_DGRAM, 0, &info->hdl);
if (ret < 0 || !info->hdl) {
pr_err("diag: In %s, socket not initialized for %s\n", __func__,
info->name);
return;
}
write_lock_bh(&info->hdl->sk->sk_callback_lock);
info->hdl->sk->sk_user_data = (void *)(info);
info->hdl->sk->sk_data_ready = socket_data_ready;
info->hdl->sk->sk_write_space = socket_flow_cntl;
write_unlock_bh(&info->hdl->sk->sk_callback_lock);
srv_addr.family = AF_MSM_IPC;
srv_addr.address.addrtype = MSM_IPC_ADDR_NAME;
srv_addr.address.addr.port_name.service = info->svc_id;
srv_addr.address.addr.port_name.instance = info->ins_id;
ret = kernel_bind(info->hdl, (struct sockaddr *)&srv_addr,
sizeof(srv_addr));
if (ret) {
pr_err("diag: In %s, failed to bind, ch: %s, svc_id: %d ins_id: %d, err: %d\n",
__func__, info->name, info->svc_id, info->ins_id, ret);
return;
}
DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s opened server svc: %d ins: %d",
info->name, info->svc_id, info->ins_id);
}
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;
}
static int diag_smd_write_ext(struct diag_smd_info *smd_info,
unsigned char *buf, int len)
{
int err = 0;
int offset = 0;
int write_len = 0;
int retry_count = 0;
int max_retries = 3;
uint8_t avail = 0;
if (!smd_info || !buf || len <= 0) {
pr_err_ratelimited("diag: In %s, invalid params, smd_info: %p, buf: %p, len: %d\n",
__func__, smd_info, buf, len);
return -EINVAL;
}
if (!smd_info->inited || !smd_info->hdl ||
!atomic_read(&smd_info->opened))
return -ENODEV;
mutex_lock(&smd_info->lock);
err = smd_write_start(smd_info->hdl, len);
if (err) {
pr_err_ratelimited("diag: In %s, error calling smd_write_start, peripheral: %d, err: %d\n",
__func__, smd_info->peripheral, err);
goto fail;
}
while (offset < len) {
retry_count = 0;
do {
if (smd_write_segment_avail(smd_info->hdl)) {
avail = 1;
break;
}
/*
* The channel maybe busy - the FIFO can be full. Retry
* after sometime. The value of 10000 was chosen
* emprically as the optimal value for the peripherals
* to read data from the SMD channel.
*/
usleep_range(10000, 10100);
retry_count++;
} while (retry_count < max_retries);
if (!avail) {
err = -EAGAIN;
goto fail;
}
write_len = smd_write_segment(smd_info->hdl, buf + offset,
(len - offset));
offset += write_len;
write_len = 0;
}
err = smd_write_end(smd_info->hdl);
if (err) {
pr_err_ratelimited("diag: In %s, error calling smd_write_end, peripheral: %d, err: %d\n",
__func__, smd_info->peripheral, err);
goto fail;
}
fail:
mutex_unlock(&smd_info->lock);
DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
"%s wrote to channel, write_len: %d, err: %d\n",
smd_info->name, offset, err);
return err;
}
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;
}
