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; }