int diag_md_close_peripheral(int id, uint8_t peripheral)
{
int i;
uint8_t found = 0;
unsigned long flags;
struct diag_md_info *ch = NULL;
struct diag_buf_tbl_t *entry = NULL;
if (id < 0 || id >= NUM_DIAG_MD_DEV || id >= DIAG_NUM_PROC)
return -EINVAL;
ch = &diag_md[id];
spin_lock_irqsave(&ch->lock, flags);
for (i = 0; i < ch->num_tbl_entries && !found; i++) {
entry = &ch->tbl[i];
if (GET_BUF_PERIPHERAL(entry->ctx) != peripheral)
continue;
found = 1;
if (ch->ops && ch->ops->write_done) {
ch->ops->write_done(entry->buf, entry->len,
entry->ctx,
DIAG_MEMORY_DEVICE_MODE);
entry->buf = NULL;
entry->len = 0;
entry->ctx = 0;
}
}
spin_unlock_irqrestore(&ch->lock, flags);
return 0;
}
int diag_mux_write(int proc, unsigned char *buf, int len, int ctx)
{
#ifdef CONFIG_LGE_DM_APP
int i;
uint8_t found = 0;
unsigned long flags;
struct diag_usb_info *usb_info = NULL;
#endif
if (proc < 0 || proc >= NUM_MUX_PROC)
return -EINVAL;
#ifdef CONFIG_LGE_DM_APP
if (driver->logging_mode == DM_APP_MODE) {
/* only diag cmd #250 for supporting testmode tool */
if ((GET_BUF_PERIPHERAL(ctx) == APPS_DATA) && (*((char *)buf) == 0xFA)) {
usb_info = &diag_usb[lge_dm_tty->id];
lge_dm_tty->dm_usb_req = diagmem_alloc(driver, sizeof(struct diag_request),
usb_info->mempool);
if (lge_dm_tty->dm_usb_req) {
lge_dm_tty->dm_usb_req->buf = buf;
lge_dm_tty->dm_usb_req->length = len;
lge_dm_tty->dm_usb_req->context = (void *)(uintptr_t)ctx;
queue_work(lge_dm_tty->dm_wq,
&(lge_dm_tty->dm_usb_work));
flush_work(&(lge_dm_tty->dm_usb_work));
}
return 0;
}
for (i = 0; i < lge_dm_tty->num_tbl_entries && !found; i++) {
spin_lock_irqsave(&lge_dm_tty->tbl[i].lock, flags);
if (lge_dm_tty->tbl[i].len == 0) {
lge_dm_tty->tbl[i].buf = buf;
lge_dm_tty->tbl[i].len = len;
lge_dm_tty->tbl[i].ctx = ctx;
found = 1;
diag_ws_on_read(DIAG_WS_MD, len);
}
spin_unlock_irqrestore(&lge_dm_tty->tbl[i].lock, flags);
}
lge_dm_tty->set_logging = 1;
wake_up_interruptible(&lge_dm_tty->waitq);
return 0;
}
#endif
#ifdef CONFIG_LGE_DIAG_BYPASS
if(diag_bypass_response(buf, len, proc, ctx, logger) > 0) {
return 0;
}
#endif
if (logger && logger->log_ops && logger->log_ops->write)
return logger->log_ops->write(proc, buf, len, ctx);
return 0;
}
int diag_md_copy_to_user(char __user *buf, int *pret, size_t buf_size,
struct diag_md_session_t *info)
{
int i, j;
int err = 0;
int ret = *pret;
int num_data = 0;
int remote_token;
unsigned long flags;
struct diag_md_info *ch = NULL;
struct diag_buf_tbl_t *entry = NULL;
uint8_t drain_again = 0;
uint8_t peripheral = 0;
struct diag_md_session_t *session_info = NULL;
for (i = 0; i < NUM_DIAG_MD_DEV && !err; i++) {
ch = &diag_md[i];
for (j = 0; j < ch->num_tbl_entries && !err; j++) {
entry = &ch->tbl[j];
if (entry->len <= 0)
continue;
peripheral = GET_BUF_PERIPHERAL(entry->ctx);
if (peripheral > NUM_PERIPHERALS)
goto drop_data;
session_info =
diag_md_session_get_peripheral(peripheral);
if (session_info && info &&
(session_info->pid != info->pid))
continue;
if ((info && (info->peripheral_mask &
MD_PERIPHERAL_MASK(peripheral)) == 0))
goto drop_data;
if (i > 0) {
if ((ret + (3 * sizeof(int)) + entry->len) >=
buf_size) {
drain_again = 1;
break;
}
} else {
if ((ret + (2 * sizeof(int)) + entry->len) >=
buf_size) {
drain_again = 1;
break;
}
}
if (i > 0) {
remote_token = diag_get_remote(i);
err = copy_to_user(buf + ret, &remote_token,
sizeof(int));
if (err)
goto drop_data;
ret += sizeof(int);
}
err = copy_to_user(buf + ret, (void *)&(entry->len),
sizeof(int));
if (err)
goto drop_data;
ret += sizeof(int);
err = copy_to_user(buf + ret, (void *)entry->buf,
entry->len);
if (err)
goto drop_data;
ret += entry->len;
num_data++;
drop_data:
spin_lock_irqsave(&ch->lock, flags);
if (ch->ops && ch->ops->write_done)
ch->ops->write_done(entry->buf, entry->len,
entry->ctx,
DIAG_MEMORY_DEVICE_MODE);
diag_ws_on_copy(DIAG_WS_MUX);
entry->buf = NULL;
entry->len = 0;
entry->ctx = 0;
spin_unlock_irqrestore(&ch->lock, flags);
}
}
*pret = ret;
err = copy_to_user(buf + sizeof(int), (void *)&num_data, sizeof(int));
diag_ws_on_copy_complete(DIAG_WS_MUX);
if (drain_again)
chk_logging_wakeup();
return err;
}
int diag_md_write(int id, unsigned char *buf, int len, int ctx)
{
int i;
uint8_t found = 0;
unsigned long flags;
struct diag_md_info *ch = NULL;
uint8_t peripheral;
struct diag_md_session_t *session_info = NULL;
if (id < 0 || id >= NUM_DIAG_MD_DEV || id >= DIAG_NUM_PROC)
return -EINVAL;
if (!buf || len < 0)
return -EINVAL;
peripheral = GET_BUF_PERIPHERAL(ctx);
if (peripheral > NUM_PERIPHERALS)
return -EINVAL;
session_info = diag_md_session_get_peripheral(peripheral);
if (!session_info)
return -EIO;
ch = &diag_md[id];
spin_lock_irqsave(&ch->lock, flags);
for (i = 0; i < ch->num_tbl_entries && !found; i++) {
if (ch->tbl[i].buf != buf)
continue;
found = 1;
pr_err_ratelimited("diag: trying to write the same buffer buf: %p, ctxt: %d len: %d at i: %d back to the table, proc: %d, mode: %d\n",
buf, ctx, ch->tbl[i].len,
i, id, driver->logging_mode);
}
spin_unlock_irqrestore(&ch->lock, flags);
if (found)
return -ENOMEM;
spin_lock_irqsave(&ch->lock, flags);
for (i = 0; i < ch->num_tbl_entries && !found; i++) {
if (ch->tbl[i].len == 0) {
ch->tbl[i].buf = buf;
ch->tbl[i].len = len;
ch->tbl[i].ctx = ctx;
found = 1;
diag_ws_on_read(DIAG_WS_MUX, len);
}
}
spin_unlock_irqrestore(&ch->lock, flags);
if (!found) {
pr_err_ratelimited("diag: Unable to find an empty space in table, please reduce logging rate, proc: %d\n",
id);
return -ENOMEM;
}
found = 0;
for (i = 0; i < driver->num_clients && !found; i++) {
if ((driver->client_map[i].pid !=
session_info->pid) ||
(driver->client_map[i].pid == 0))
continue;
found = 1;
driver->data_ready[i] |= USERMODE_DIAGFWD;
DIAG_DBUG("diag: wake up logging process\n");
wake_up_interruptible(&driver->wait_q);
}
if (!found)
return -EINVAL;
return 0;
}