static int smd_vt_release(struct inode *ip, struct file *fp)
{
int r = 0;
int sz;
unsigned long flags;
printk(KERN_INFO "smd_vt_release\n");
mutex_lock(&smd_vt_ch_lock);
smd_vt_devp->open_count--;
if ((smd_vt_devp->open_count == 0) && (smd_vt_devp->ch != 0)) {
mutex_lock(&smd_vt_rx_buf_lock);
spin_lock_irqsave(&smd_vt_read_lock, flags);
smd_vt_devp->read_avail = 0;
spin_unlock_irqrestore(&smd_vt_read_lock, flags);
sz = smd_cur_packet_size(smd_vt_devp->ch);
while((sz != 0) && (sz <= smd_read_avail(smd_vt_devp->ch))) {
if (sz > MAX_RX_BUF_SIZE) {
smd_read(smd_vt_devp->ch, smd_vt_devp->rx_buf, MAX_RX_BUF_SIZE);
sz= sz -MAX_RX_BUF_SIZE;
}
else{
smd_read(smd_vt_devp->ch, smd_vt_devp->rx_buf, sz);
sz =0;
}
}
mutex_unlock(&smd_vt_rx_buf_lock);
r = smd_close(smd_vt_devp->ch);
smd_vt_devp->ch = 0;
smd_vt_devp->open_flag=0;
}
mutex_unlock(&smd_vt_ch_lock);
return r;
}
void __diag_smd_wcnss_send_req(void)
{
void *buf = driver->buf_in_wcnss;
int *in_busy_wcnss_ptr = &(driver->in_busy_wcnss);
struct diag_request *write_ptr_wcnss = driver->write_ptr_wcnss;
#if DIAG_XPST
int type;
#endif
if ((!driver->in_busy_wcnss) && driver->ch_wcnss && buf) {
int r = smd_read_avail(driver->ch_wcnss);
if (r > IN_BUF_SIZE) {
if (r < MAX_IN_BUF_SIZE) {
pr_err("diag: wcnss packets > %d bytes", r);
buf = krealloc(buf, r, GFP_KERNEL);
} else {
pr_err("diag: wcnss pkt > %d", MAX_IN_BUF_SIZE);
return;
}
}
if (r > 0) {
if (!buf) {
pr_err("Out of diagmem for wcnss\n");
} else {
APPEND_DEBUG('i');
smd_read(driver->ch_wcnss, buf, r);
APPEND_DEBUG('j');
#if DIAG_XPST
type = checkcmd_modem_epst(buf);
if (type) {
modem_to_userspace(buf, r, type, 0);
return;
}
#endif
write_ptr_wcnss->length = r;
*in_busy_wcnss_ptr = 1;
diag_device_write(buf, WCNSS_DATA,
write_ptr_wcnss);
}
}
}
}
void __diag_smd_qdsp_send_req(int context)
{
void *buf;
if (driver->chqdsp && (!driver->in_busy_qdsp)) {
int r = smd_read_avail(driver->chqdsp);
if (r > USB_MAX_IN_BUF) {
printk(KERN_INFO "diag dropped num bytes = %d\n", r);
return;
}
if (r > 0) {
buf = driver->usb_buf_in_qdsp;
if (!buf) {
printk(KERN_INFO "Out of diagmem for q6\n");
} else {
APPEND_DEBUG('l');
if (context == SMD_CONTEXT)
smd_read_from_cb(
driver->chqdsp, buf, r);
else
smd_read(driver->chqdsp, buf, r);
printk(KERN_DEBUG "[LGE] diag_smd_qdsp_send_req()\n");
if (g_testmode == 1) {
printk("[LGE] QDSP req is ignored\n");
g_testmode = 0;
return;
}
printk("[LGE] QDSP req is accepted\n");
APPEND_DEBUG('m');
driver->usb_write_ptr_qdsp->length = r;
driver->in_busy_qdsp = 1;
diag_device_write(buf, QDSP_DATA);
}
}
}
}
static void grmnet_ctrl_smd_notify(void *p, unsigned event)
{
struct rmnet_ctrl_port *port = p;
struct smd_ch_info *c = &port->ctrl_ch;
struct rmnet_ctrl_pkt *cpkt;
unsigned long flags;
pr_debug("%s: EVENT_(%s)\n", __func__, get_smd_event(event));
switch (event) {
case SMD_EVENT_DATA:
if (smd_read_avail(c->ch))
queue_work(grmnet_ctrl_wq, &c->read_w);
if (smd_write_avail(c->ch))
queue_work(grmnet_ctrl_wq, &c->write_w);
break;
case SMD_EVENT_OPEN:
set_bit(CH_OPENED, &c->flags);
if (port && port->port_usb && port->port_usb->connect)
port->port_usb->connect(port->port_usb);
break;
case SMD_EVENT_CLOSE:
clear_bit(CH_OPENED, &c->flags);
if (port && port->port_usb && port->port_usb->disconnect)
port->port_usb->disconnect(port->port_usb);
spin_lock_irqsave(&port->port_lock, flags);
while (!list_empty(&c->tx_q)) {
cpkt = list_first_entry(&c->tx_q,
struct rmnet_ctrl_pkt, list);
list_del(&cpkt->list);
free_rmnet_ctrl_pkt(cpkt);
}
spin_unlock_irqrestore(&port->port_lock, flags);
break;
}
}
static unsigned char wcnss_fw_status(void)
{
int len = 0;
int rc = 0;
unsigned char fw_status = 0xFF;
len = smd_read_avail(penv->smd_ch);
if (len < 1) {
pr_err("%s: invalid firmware status", __func__);
return fw_status;
}
rc = smd_read(penv->smd_ch, &fw_status, 1);
if (rc < 0) {
pr_err("%s: incomplete data read from smd\n", __func__);
return fw_status;
}
return fw_status;
}
static void hci_smd_notify_event(void *data, unsigned int event)
{
struct hci_dev *hdev = hs.hdev;
struct hci_smd_data *hsmd = &hs;
struct work_struct *reset_worker;
int len = 0;
if (!hdev) {
BT_ERR("Frame for unknown HCI device (hdev=NULL)");
return;
}
switch (event) {
case SMD_EVENT_DATA:
len = smd_read_avail(hsmd->event_channel);
if (len > 0)
tasklet_hi_schedule(&hs.rx_task);
else if (len < 0)
BT_ERR("Failed to read event from smd %d", len);
break;
case SMD_EVENT_OPEN:
BT_INFO("opening HCI-SMD channel :%s", EVENT_CHANNEL);
hci_smd_open(hdev);
break;
case SMD_EVENT_CLOSE:
BT_INFO("Closing HCI-SMD channel :%s", EVENT_CHANNEL);
hci_smd_close(hdev);
reset_worker = kzalloc(sizeof(*reset_worker), GFP_ATOMIC);
if (!reset_worker) {
BT_ERR("Out of memory");
break;
}
INIT_WORK(reset_worker, hci_dev_restart);
schedule_work(reset_worker);
break;
default:
break;
}
}
static void smd_try_to_send(struct diag_context *ctxt)
{
if (ctxt->ch) {
int r = smd_read_avail(ctxt->ch);
if (r > RXN_MAX) {
printk(KERN_ERR "The SMD data is too large to send (%d) !!\n", r);
r = RXN_MAX;
}
if (r > 0) {
struct diag_request *req = get_req(ctxt, &ctxt->rx_arm9_idle);
if (!req) {
printk(KERN_ERR "There is no enough request to ARM11!!\n");
return;
}
smd_read(ctxt->ch, req->buf, r);
smd_xfer_count_func(r, data_set_rx);
req->actual = r;
put_req(ctxt, &ctxt->rx_arm9_done, req);
wake_up(&ctxt->read_arm9_wq);
}
}
}
static void grmnet_ctrl_smd_read_w(struct work_struct *w)
{
struct smd_ch_info *c = container_of(w, struct smd_ch_info, read_w);
struct rmnet_ctrl_port *port = c->port;
int sz;
size_t len;
void *buf;
unsigned long flags;
spin_lock_irqsave(&port->port_lock, flags);
while (c->ch) {
if (c->ch == NULL)
break;
sz = smd_cur_packet_size(c->ch);
if (sz <= 0)
break;
if (smd_read_avail(c->ch) < sz)
break;
spin_unlock_irqrestore(&port->port_lock, flags);
buf = kmalloc(sz, GFP_KERNEL);
if (!buf)
return;
len = smd_read(c->ch, buf, sz);
/* send it to USB here */
spin_lock_irqsave(&port->port_lock, flags);
if (port->port_usb && port->port_usb->send_cpkt_response) {
port->port_usb->send_cpkt_response(port->port_usb,
buf, len);
c->to_host++;
}
kfree(buf);
}
spin_unlock_irqrestore(&port->port_lock, flags);
}
void __diag_smd_send_req(int context)
{
void *buf;
if (driver->ch && (!driver->in_busy)) {
int r = smd_read_avail(driver->ch);
if (r > USB_MAX_IN_BUF) {
if (r < MAX_BUF_SIZE) {
printk(KERN_ALERT "\n diag: SMD sending in "
"packets upto %d bytes", r);
driver->usb_buf_in = krealloc(
driver->usb_buf_in, r, GFP_KERNEL);
} else {
printk(KERN_ALERT "\n diag: SMD sending in "
"packets more than %d bytes", MAX_BUF_SIZE);
return;
}
}
if (r > 0) {
buf = driver->usb_buf_in;
if (!buf) {
printk(KERN_INFO "Out of diagmem for a9\n");
} else {
APPEND_DEBUG('i');
if (context == SMD_CONTEXT)
smd_read_from_cb(driver->ch, buf, r);
else
smd_read(driver->ch, buf, r);
APPEND_DEBUG('j');
driver->usb_write_ptr->length = r;
driver->in_busy = 1;
diag_device_write(buf, MODEM_DATA);
}
}
}
}
static void wcn36xx_smd_work(struct work_struct *work)
{
int msg_len;
int avail;
void *msg;
int ret;
struct wcn36xx *wcn =
container_of(work, struct wcn36xx, smd_work);
if (!wcn)
return;
while (1) {
msg_len = smd_cur_packet_size(wcn->smd_ch);
if (0 == msg_len) {
complete(&wcn->smd_compl);
return;
}
avail = smd_read_avail(wcn->smd_ch);
if (avail < msg_len) {
complete(&wcn->smd_compl);
return;
}
msg = kmalloc(msg_len, GFP_KERNEL);
if (NULL == msg) {
complete(&wcn->smd_compl);
return;
}
ret = smd_read(wcn->smd_ch, msg, msg_len);
if (ret != msg_len) {
complete(&wcn->smd_compl);
return;
}
wcn36xx_smd_rsp_process(wcn, msg, msg_len);
kfree(msg);
}
}
static void qmi_notify(void *priv, unsigned event)
{
struct qmi_ctxt *ctxt = priv;
switch (event) {
case SMD_EVENT_DATA: {
int sz;
sz = smd_cur_packet_size(ctxt->ch);
if ((sz > 0) && (sz <= smd_read_avail(ctxt->ch))) {
wake_lock_timeout(&ctxt->wake_lock, HZ / 2);
queue_work(qmi_wq, &ctxt->read_work);
}
break;
}
case SMD_EVENT_OPEN:
printk(KERN_INFO "qmi: smd opened\n");
queue_work(qmi_wq, &ctxt->open_work);
break;
case SMD_EVENT_CLOSE:
printk(KERN_INFO "qmi: smd closed\n");
break;
}
}
static void grmnet_ctrl_smd_notify(void *p, unsigned event)
{
struct rmnet_ctrl_port *port = p;
struct smd_ch_info *c = &port->ctrl_ch;
pr_debug("%s: EVENT_(%s)\n", __func__, get_smd_event(event));
switch (event) {
case SMD_EVENT_DATA:
if (smd_read_avail(c->ch))
queue_work(grmnet_ctrl_wq, &c->read_w);
if (smd_write_avail(c->ch))
queue_work(grmnet_ctrl_wq, &c->write_w);
break;
case SMD_EVENT_OPEN:
set_bit(CH_OPENED, &c->flags);
wake_up(&c->wait);
break;
case SMD_EVENT_CLOSE:
clear_bit(CH_OPENED, &c->flags);
break;
}
}
void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event)
{
int r1, r2;
if (!(driver->ch_wcnss_cntl))
return;
switch (event) {
case SMD_EVENT_DATA:
r1 = smd_read_avail(driver->ch_wcnss_cntl);
r2 = smd_cur_packet_size(driver->ch_wcnss_cntl);
if (r1 > 0 && r1 == r2)
queue_work(driver->diag_wq,
&(driver->diag_read_smd_wcnss_cntl_work));
else
pr_debug("diag: incomplete pkt on WCNSS CNTL ch\n");
break;
case SMD_EVENT_OPEN:
queue_work(driver->diag_cntl_wq,
&(driver->diag_wcnss_mask_update_work));
break;
}
}
static void wcnss_smd_notify_event(void *data, unsigned int event)
{
int len = 0;
if (penv != data) {
pr_err("wcnss: invalid env pointer in smd callback\n");
return;
}
switch (event) {
case SMD_EVENT_DATA:
len = smd_read_avail(penv->smd_ch);
if (len < 0) {
pr_err("wcnss: failed to read from smd %d\n", len);
return;
}
schedule_work(&penv->wcnssctrl_rx_work);
break;
case SMD_EVENT_OPEN:
pr_debug("wcnss: opening WCNSS SMD channel :%s",
WCNSS_CTRL_CHANNEL);
schedule_work(&penv->wcnssctrl_version_work);
break;
case SMD_EVENT_CLOSE:
pr_debug("wcnss: closing WCNSS SMD channel :%s",
WCNSS_CTRL_CHANNEL);
/* This SMD is closed only during SSR */
penv->ssr_boot = true;
penv->nv_downloaded = 0;
break;
default:
break;
}
}
static void ssm_app_modem_work_fn(struct work_struct *work)
{
int sz, rc;
struct ssm_common_msg pkt;
struct ssm_driver *ssm;
ssm = container_of(work, struct ssm_driver, ipc_work);
mutex_lock(&ssm->mutex);
sz = smd_cur_packet_size(ssm->ch);
if ((sz < SSM_MSG_FIELD_LEN) || (sz > ATOM_MSG_LEN)) {
dev_dbg(ssm_drv->dev, "Garbled message size\n");
goto unlock;
}
if (smd_read_avail(ssm->ch) < sz) {
dev_err(ssm_drv->dev, "SMD error data in channel\n");
goto unlock;
}
if (smd_read(ssm->ch, ssm->smd_buffer, sz) != sz) {
dev_err(ssm_drv->dev, "Incomplete data\n");
goto unlock;
}
rc = decode_packet(ssm->smd_buffer, &pkt);
if (rc < 0) {
dev_err(ssm_drv->dev, "Corrupted header\n");
goto unlock;
}
process_message(pkt, ssm);
unlock:
mutex_unlock(&ssm->mutex);
}
static void smd_tty_read(unsigned long param)
{
unsigned char *ptr;
int avail;
struct smd_tty_info *info = (struct smd_tty_info *)param;
struct tty_struct *tty = info->tty;
if (!tty)
return;
for (;;) {
if (test_bit(TTY_THROTTLED, &tty->flags)) break;
avail = smd_read_avail(info->ch);
if (avail == 0)
break;
if (avail > MAX_TTY_BUF_SIZE)
avail = MAX_TTY_BUF_SIZE;
avail = tty_prepare_flip_string(tty, &ptr, avail);
if (smd_read(info->ch, ptr, avail) != avail) {
/* shouldn't be possible since we're in interrupt
** context here and nobody else could 'steal' our
** characters.
*/
printk(KERN_ERR "OOPS - smd_tty_buffer mismatch?!");
}
wake_lock_timeout(&info->wake_lock, HZ / 2);
tty_flip_buffer_push(tty);
}
/* XXX only when writable and necessary */
tty_wakeup(tty);
}
void __diag_smd_qdsp_send_req(void)
{
void *buf = NULL;
int *in_busy_qdsp_ptr = NULL;
struct diag_request *write_ptr_qdsp = NULL;
#if DIAG_XPST
int type;
#endif
if (!driver->in_busy_qdsp_1) {
buf = driver->buf_in_qdsp_1;
write_ptr_qdsp = driver->write_ptr_qdsp_1;
in_busy_qdsp_ptr = &(driver->in_busy_qdsp_1);
} else if (!driver->in_busy_qdsp_2) {
buf = driver->buf_in_qdsp_2;
write_ptr_qdsp = driver->write_ptr_qdsp_2;
in_busy_qdsp_ptr = &(driver->in_busy_qdsp_2);
}
if (driver->chqdsp && buf) {
int r = smd_read_avail(driver->chqdsp);
if (r > IN_BUF_SIZE) {
if (r < MAX_IN_BUF_SIZE) {
DIAGFWD_INFO("\n diag: SMD sending in "
"packets upto %d bytes", r);
buf = krealloc(buf, r, GFP_KERNEL);
} else {
DIAGFWD_ERR("\n diag: SMD sending in "
"packets more than %d bytes", MAX_IN_BUF_SIZE);
return;
}
}
if (r > 0) {
if (!buf)
DIAGFWD_INFO("Out of diagmem for QDSP\n");
else {
/* APPEND_DEBUG('i'); */
smd_read(driver->chqdsp, buf, r);
if (diag7k_debug_mask) {
switch (diag7k_debug_mask) {
case 1:
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from qdsp(first 16 bytes)", DUMP_PREFIX_ADDRESS, 16, 1, buf, 16, 1);
break;
case 2:
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from qdsp(first 16 bytes)", DUMP_PREFIX_ADDRESS, 16, 1, buf, 16, 1);
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from qdsp(last 16 bytes) ", DUMP_PREFIX_ADDRESS, 16, 1, buf+r-16, 16, 1);
break;
default:
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from qdsp ", DUMP_PREFIX_ADDRESS, 16, 1, buf, r, 1);
}
}
#if DIAG_XPST
type = checkcmd_modem_epst(buf);
if (type) {
modem_to_userspace(buf, r, type, 0);
return;
}
#endif
/* APPEND_DEBUG('j'); */
write_ptr_qdsp->length = r;
*in_busy_qdsp_ptr = 1;
diag_device_write(buf, QDSP_DATA,
write_ptr_qdsp);
}
}
}
}
static int smd_tty_chars_in_buffer(struct tty_struct *tty)
{
struct smd_tty_info *info = tty->driver_data;
return smd_read_avail(info->ch);
}
static void diag_smd_dci_send_req(int proc_num)
{
void *buf = NULL;
smd_channel_t *smd_ch = NULL;
int i, r, found = 1;
int cmd_code_len = 1;
if (driver->in_busy_dci)
return;
if (proc_num == MODEM_PROC) {
buf = driver->buf_in_dci;
smd_ch = driver->ch_dci;
}
if (!smd_ch || !buf)
return;
r = smd_read_avail(smd_ch);
if (r > IN_BUF_SIZE) {
if (r < MAX_IN_BUF_SIZE) {
pr_err("diag: SMD DCI sending pkt upto %d bytes", r);
buf = krealloc(buf, r, GFP_KERNEL);
} else {
pr_err("diag: DCI pkt > %d bytes", MAX_IN_BUF_SIZE);
return;
}
}
if (buf && r > 0) {
smd_read(smd_ch, buf, r);
diag_printk(1,"diag:%s data received ---\n",__func__);
for (i = 0; i < r; i++)
diag_printk(1,"\t %x \t", *(((unsigned char *)buf)+i));
if (*(uint8_t *)(buf+4) != DCI_CMD_CODE)
cmd_code_len = 4; /* delayed response */
driver->write_ptr_dci->length =
(int)(*(uint16_t *)(buf+2)) - (4+cmd_code_len);
diag_printk(1,"diag:%s len = %d\n",__func__, (int)(*(uint16_t *)(buf+2))
- (4+cmd_code_len));
/* look up DCI client with tag */
for (i = 0; i < dci_max_reg; i++) {
if (driver->dci_tbl[i].tag ==
*(int *)(buf+(4+cmd_code_len))) {
found = 0;
break;
}
}
if (found)
pr_alert("diag: No matching PID for DCI data\n");
diag_printk(1,"\n diag:%s PID = %d",__func__, driver->dci_tbl[i].pid);
if (driver->dci_tbl[i].pid == 0)
pr_alert("diag: Receiving DCI process deleted\n");
*(int *)(buf+4+cmd_code_len) = driver->dci_tbl[i].uid;
/* update len after adding UID */
driver->write_ptr_dci->length =
driver->write_ptr_dci->length + 4;
diag_printk(1,"diag:%s data receivd, wake process\n",__func__);
driver->in_busy_dci = 1;
diag_update_sleeping_process(driver->dci_tbl[i].pid,
DCI_DATA_TYPE);
/* delete immediate response entry */
if (driver->buf_in_dci[8+cmd_code_len] != 0x80)
driver->dci_tbl[i].pid = 0;
for (i = 0; i < dci_max_reg; i++)
if (driver->dci_tbl[i].pid != 0)
diag_printk(1,"diag:%s PID = %d, UID = %d, tag = %d\n",
__func__,driver->dci_tbl[i].pid, driver->dci_tbl[i].uid,
driver->dci_tbl[i].tag);
diag_printk(1,"diag:%s completed clearing table\n",__func__);
}
}
/**
@brief Callback function for serializing WCTS Read
processing in the control context
@param pWCTSCb WCTS Control Block
@see
@return void
*/
static void
WCTS_PALReadCallback
(
WCTS_ControlBlockType* pWCTSCb
)
{
void* buffer;
int packet_size;
int available;
int bytes_read;
/*- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */
/*--------------------------------------------------------------------
Sanity check
--------------------------------------------------------------------*/
if ((NULL == pWCTSCb) || (WCTS_CB_MAGIC != pWCTSCb->wctsMagic)) {
WPAL_TRACE(eWLAN_MODULE_DAL_CTRL, eWLAN_PAL_TRACE_LEVEL_ERROR,
"WCTS_PALReadCallback: Invalid parameter received.");
return;
}
/* iterate until no more packets are available */
while (1) {
/* check the length of the next packet */
packet_size = smd_cur_packet_size(pWCTSCb->wctsChannel);
if (0 == packet_size) {
/* No more data to be read */
return;
}
/* Check how much of the data is available */
available = smd_read_avail(pWCTSCb->wctsChannel);
if (available < packet_size) {
/* Entire packet not yet ready to be read --
There will be another notification when it is ready */
return;
}
buffer = wpalMemoryAllocate(packet_size);
if (NULL == buffer) {
WPAL_TRACE(eWLAN_MODULE_DAL_CTRL, eWLAN_PAL_TRACE_LEVEL_ERROR,
"WCTS_PALReadCallback: Memory allocation failure");
WPAL_ASSERT(0);
return;
}
bytes_read = smd_read(pWCTSCb->wctsChannel,
buffer,
packet_size);
if (bytes_read != packet_size) {
/*Some problem, do not forward it to WDI.*/
WPAL_TRACE(eWLAN_MODULE_DAL_CTRL, eWLAN_PAL_TRACE_LEVEL_ERROR,
"WCTS_PALReadCallback: Failed to read data from SMD");
wpalMemoryFree(buffer);
WPAL_ASSERT(0);
return;
}
/* forward the message to the registered handler */
pWCTSCb->wctsRxMsgCB((WCTS_HandleType)pWCTSCb,
buffer,
packet_size,
pWCTSCb->wctsRxMsgCBData);
/* Free the allocated buffer*/
wpalMemoryFree(buffer);
}
} /*WCTS_PALReadCallback*/
void __diag_smd_send_req(void)
{
void *buf = NULL;
int *in_busy_ptr = NULL;
struct diag_request *write_ptr_modem = NULL;
if (!driver->in_busy_1) {
buf = driver->buf_in_1;
write_ptr_modem = driver->write_ptr_1;
in_busy_ptr = &(driver->in_busy_1);
} else if (!driver->in_busy_2) {
buf = driver->buf_in_2;
write_ptr_modem = driver->write_ptr_2;
in_busy_ptr = &(driver->in_busy_2);
}
if (driver->ch && buf) {
int r = smd_read_avail(driver->ch);
if (r > IN_BUF_SIZE) {
if (r < MAX_IN_BUF_SIZE) {
pr_err("diag: SMD sending in "
"packets upto %d bytes", r);
buf = krealloc(buf, r, GFP_KERNEL);
} else {
pr_err("diag: SMD sending in "
"packets more than %d bytes", MAX_IN_BUF_SIZE);
return;
}
}
if (r > 0) {
if (!buf)
pr_info("Out of diagmem for Modem\n");
else {
#ifdef CONFIG_HUAWEI_FEATURE_PHUDIAG
mutex_lock(&phudriver->diagchar_mutex);
if(phudriver->opened)
{
if(r != phudiagfwd_ring_buf_set_data_before_process(phudriver->in_buf, r))
{
printk(KERN_INFO "__diag_smd_send_req write in_buf out of memory !\n");
mutex_unlock(&phudriver->diagchar_mutex);
return;
}
}
#endif
APPEND_DEBUG('i');
smd_read(driver->ch, buf, r);
APPEND_DEBUG('j');
#ifdef CONFIG_HUAWEI_FEATURE_PHUDIAG
if(phudriver->opened)
{
memcpy(phudriver->in_buf->end,buf,r);
phudriver->in_buf->end += r;
}
mutex_unlock(&phudriver->diagchar_mutex);
#endif
write_ptr_modem->length = r;
*in_busy_ptr = 1;
diag_device_write(buf, MODEM_DATA,
write_ptr_modem);
}
}
}
#ifdef CONFIG_HUAWEI_FEATURE_PHUDIAG
phudiagfwd_usb_diag_suspend_packet_num++;
mutex_lock(&phudriver->diagchar_mutex);
if(phudriver->opened && NULL == buf
&& phudiagfwd_usb_diag_suspend_packet_num > 10 )
{
phudiagfwd_usb_diag_suspend_packet_num = 10;
phudiagfwd_read_data_from_smd();
}
mutex_unlock(&phudriver->diagchar_mutex);
#endif
}
static int rpcrouter_smd_remote_read_avail(void)
{
return smd_read_avail(smd_remote_xprt.channel);
}
static int rpcrouter_smd_loopback_read_avail(void)
{
return smd_read_avail(smd_loopback_xprt.channel);
}
static ssize_t smd_vt_read(struct file *fp, char __user *buf,
size_t count, loff_t *pos)
{
int r = 0;
int bytes_read = 0;
unsigned long flags;
int sz, avail;
if(count > MAX_RX_BUF_SIZE) count = MAX_RX_BUF_SIZE;
for (;;) {
mutex_lock(&smd_vt_rx_buf_lock);
if( (smd_vt_devp->open_count == 0) || (smd_vt_devp->ch == 0)
|| (smd_vt_devp->open_flag==0) || (count <=0)){
mutex_unlock(&smd_vt_rx_buf_lock);
return 0;
}
spin_lock_irqsave(&smd_vt_read_lock, flags);
smd_vt_devp->read_avail = 0;
spin_unlock_irqrestore(&smd_vt_read_lock, flags);
sz = smd_cur_packet_size(smd_vt_devp->ch);
if (sz > MAX_RX_BUF_SIZE) { // garbage data
smd_read(smd_vt_devp->ch, 0, sz);
mutex_unlock(&smd_vt_rx_buf_lock);
continue;
}
avail = smd_read_avail(smd_vt_devp->ch);
if((sz == avail) && (sz != 0)){
if( sz > count) sz = count; // output bffer size check.
bytes_read = smd_read(smd_vt_devp->ch, buf, sz);
break;
}else if( sz < avail ){ // garbage data
smd_read(smd_vt_devp->ch, 0, avail);
mutex_unlock(&smd_vt_rx_buf_lock);
continue;
}
// else is data not ready wait data.
mutex_unlock(&smd_vt_rx_buf_lock);
r = wait_event_interruptible_timeout(smd_vt_wait_queue,
smd_vt_devp->read_avail,HZ*5);
if (r <= 0) { // timeout or error
/* qualify error message */
if (r != -ERESTARTSYS) {
/* we get this anytime a signal comes in */
printk(KERN_ERR "ERROR:%s:%i:%s: "
"wait_event_interruptible ret %i\n",
__FILE__,
__LINE__,
__func__,
r
);
}
return 0; //r;
}
}
mutex_unlock(&smd_vt_rx_buf_lock);
return bytes_read;
}
/* Called in soft-irq context */
static void smd_net_data_handler(unsigned long arg)
{
struct net_device *dev = (struct net_device *) arg;
struct rmnet_private *p = netdev_priv(dev);
struct sk_buff *skb;
void *ptr = 0;
int sz;
u32 opmode = p->operation_mode;
// unsigned long flags;
// int max_package_size;
for (;;) {
sz = smd_cur_packet_size(p->ch);
if (sz == 0) break;
if (smd_read_avail(p->ch) < sz) break;
//ZTE_RIL_WANGCHENG_20110425 start
#ifdef CONFIG_ZTE_PLATFORM
if (RMNET_IS_MODE_IP(opmode) ? (sz > ((dev->mtu > RMNET_DEFAULT_MTU_LEN)? dev->mtu:RMNET_DEFAULT_MTU_LEN)) :
(sz > (((dev->mtu > RMNET_DEFAULT_MTU_LEN)? dev->mtu:RMNET_DEFAULT_MTU_LEN) + ETH_HLEN))) {
#else
if (RMNET_IS_MODE_IP(opmode) ? (sz > dev->mtu) :
(sz > (dev->mtu + ETH_HLEN))) {
#endif
pr_err("rmnet_recv() discarding %d len (%d mtu)\n",
sz, RMNET_IS_MODE_IP(opmode) ?
dev->mtu : (dev->mtu + ETH_HLEN));
ptr = 0;
} else {
skb = dev_alloc_skb(sz + NET_IP_ALIGN);
if (skb == NULL) {
pr_err("rmnet_recv() cannot allocate skb\n");
} else {
skb->dev = dev;
skb_reserve(skb, NET_IP_ALIGN);
ptr = skb_put(skb, sz);
wake_lock_timeout(&p->wake_lock, HZ / 2);
if (smd_read(p->ch, ptr, sz) != sz) {
pr_err("rmnet_recv() smd lied about avail?!");
ptr = 0;
dev_kfree_skb_irq(skb);
} else {
/* Handle Rx frame format */
//spin_lock_irqsave(&p->lock, flags);
//opmode = p->operation_mode;
//spin_unlock_irqrestore(&p->lock, flags);
if (RMNET_IS_MODE_IP(opmode)) {
/* Driver in IP mode */
skb->protocol =
rmnet_ip_type_trans(skb, dev);
} else {
/* Driver in Ethernet mode */
skb->protocol =
eth_type_trans(skb, dev);
}
if (RMNET_IS_MODE_IP(opmode) ||
count_this_packet(ptr, skb->len)) {
#ifdef CONFIG_MSM_RMNET_DEBUG
p->wakeups_rcv +=
rmnet_cause_wakeup(p);
#endif
p->stats.rx_packets++;
p->stats.rx_bytes += skb->len;
}
netif_rx(skb);
}
continue;
}
}
if (smd_read(p->ch, ptr, sz) != sz)
pr_err("rmnet_recv() smd lied about avail?!");
}
}
//ZTE_RIL_RJG_20101103 end
static DECLARE_TASKLET(smd_net_data_tasklet, smd_net_data_handler, 0);
static int _rmnet_xmit(struct sk_buff *skb, struct net_device *dev)
{
struct rmnet_private *p = netdev_priv(dev);
smd_channel_t *ch = p->ch;
int smd_ret;
struct QMI_QOS_HDR_S *qmih;
u32 opmode;
unsigned long flags;
/* For QoS mode, prepend QMI header and assign flow ID from skb->mark */
spin_lock_irqsave(&p->lock, flags);
opmode = p->operation_mode;
spin_unlock_irqrestore(&p->lock, flags);
if (RMNET_IS_MODE_QOS(opmode)) {
qmih = (struct QMI_QOS_HDR_S *)
skb_push(skb, sizeof(struct QMI_QOS_HDR_S));
qmih->version = 1;
qmih->flags = 0;
qmih->flow_id = skb->mark;
}
dev->trans_start = jiffies;
smd_ret = smd_write(ch, skb->data, skb->len);
if (smd_ret != skb->len) {
pr_err("%s: smd_write returned error %d", __func__, smd_ret);
goto xmit_out;
}
if (RMNET_IS_MODE_IP(opmode) ||
count_this_packet(skb->data, skb->len)) {
p->stats.tx_packets++;
p->stats.tx_bytes += skb->len;
#ifdef CONFIG_MSM_RMNET_DEBUG
p->wakeups_xmit += rmnet_cause_wakeup(p);
#endif
}
xmit_out:
/* data xmited, safe to release skb */
dev_kfree_skb_irq(skb);
return 0;
}
static void _rmnet_resume_flow(unsigned long param)
{
struct net_device *dev = (struct net_device *)param;
struct rmnet_private *p = netdev_priv(dev);
struct sk_buff *skb = NULL;
unsigned long flags;
/* xmit and enable the flow only once even if
multiple tasklets were scheduled by smd_net_notify */
spin_lock_irqsave(&p->lock, flags);
if (p->skb && (smd_write_avail(p->ch) >= p->skb->len)) {
skb = p->skb;
p->skb = NULL;
spin_unlock_irqrestore(&p->lock, flags);
_rmnet_xmit(skb, dev);
netif_wake_queue(dev);
} else
spin_unlock_irqrestore(&p->lock, flags);
}
static void msm_rmnet_unload_modem(void *pil)
{
if (pil)
pil_put(pil);
}
static void *msm_rmnet_load_modem(struct net_device *dev)
{
void *pil;
int rc;
struct rmnet_private *p = netdev_priv(dev);
pil = pil_get("modem");
if (IS_ERR(pil))
pr_err("%s: modem load failed\n", __func__);
else if (msm_rmnet_modem_wait) {
rc = wait_for_completion_interruptible_timeout(
&p->complete,
msecs_to_jiffies(msm_rmnet_modem_wait * 1000));
if (!rc)
rc = -ETIMEDOUT;
if (rc < 0) {
pr_err("%s: wait for rmnet port failed %d\n",
__func__, rc);
msm_rmnet_unload_modem(pil);
pil = ERR_PTR(rc);
}
}
return pil;
}
void __diag_smd_send_req(void)
{
void *buf = NULL;
int *in_busy_ptr = NULL;
struct diag_request *write_ptr_modem = NULL;
#if DIAG_XPST
int type;
#endif
#ifdef SDQXDM_DEBUG
static struct timeval t0 = {0, 0}, t1;
static int full = 0, empty = 0;
long diff;
#endif
if (!driver->in_busy_1) {
buf = driver->buf_in_1;
write_ptr_modem = driver->write_ptr_1;
in_busy_ptr = &(driver->in_busy_1);
} else if (!driver->in_busy_2) {
buf = driver->buf_in_2;
write_ptr_modem = driver->write_ptr_2;
in_busy_ptr = &(driver->in_busy_2);
}
if (driver->ch && buf) {
int r = smd_read_avail(driver->ch);
if (r > IN_BUF_SIZE) {
if (r < MAX_IN_BUF_SIZE) {
DIAGFWD_INFO("\n diag: SMD sending in "
"packets upto %d bytes", r);
buf = krealloc(buf, r, GFP_KERNEL);
} else {
DIAGFWD_ERR("\n diag: SMD sending in "
"packets more than %d bytes", MAX_IN_BUF_SIZE);
return;
}
}
if (r > 0) {
if (!buf)
DIAGFWD_INFO("Out of diagmem for a9\n");
else {
/* APPEND_DEBUG('i'); */
smd_read(driver->ch, buf, r);
if (diag7k_debug_mask) {
switch (diag7k_debug_mask) {
case 1:
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from 7K(first 16 bytes)", DUMP_PREFIX_ADDRESS, 16, 1, buf, 16, 1);
break;
case 2:
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from 7K(first 16 bytes)", DUMP_PREFIX_ADDRESS, 16, 1, buf, 16, 1);
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from 7K(last 16 bytes) ", DUMP_PREFIX_ADDRESS, 16, 1, buf+r-16, 16, 1);
break;
default:
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from 7K ", DUMP_PREFIX_ADDRESS, 16, 1, buf, r, 1);
}
}
#if DIAG_XPST
type = checkcmd_modem_epst(buf);
if (type) {
modem_to_userspace(buf, r, type, 0);
return;
}
#endif
#ifdef SDQXDM_DEBUG
if (full) {
pr_err("[diag-dbg] buffer become available %d %d, read %d\n", driver->in_busy_1, driver->in_busy_2, r);
full = 0;
}
do_gettimeofday(&t1);
diff = (t1.tv_sec-t0.tv_sec)*1000 + (t1.tv_usec-t0.tv_usec)/1000;
if (diff > 1000) {
pr_err("[diag-dbg] Over time (%ld) %ld.%04ld -> %ld.%04ld empty = %d\n", diff, (long)t0.tv_sec, t0.tv_usec/1000, (long)t1.tv_sec, t1.tv_usec/1000, empty);
}
write_ptr_modem->second = t1.tv_sec;
t0 = t1;
empty = 0;
#endif
/* APPEND_DEBUG('j'); */
write_ptr_modem->length = r;
*in_busy_ptr = 1;
diag_device_write(buf, MODEM_DATA,
write_ptr_modem);
}
}
#ifdef SDQXDM_DEBUG
else {
empty++;
}
#endif
}
else {
#ifdef SDQXDM_DEBUG
if (!full && driver->ch)
pr_info("[diag-dbg] Buffer full, %d bytes pending.\n", smd_read_avail(driver->ch));
full = 1;
#endif
wake_lock_timeout(&driver->wake_lock, HZ);
}
}
/* Called in soft-irq context */
static void smd_net_data_handler(unsigned long arg)
{
struct net_device *dev = (struct net_device *) arg;
struct rmnet_private *p = netdev_priv(dev);
struct sk_buff *skb;
void *ptr = 0;
int sz;
u32 opmode = p->operation_mode;
unsigned long flags;
for (;;) {
sz = smd_cur_packet_size(p->ch);
if (sz == 0) break;
if (smd_read_avail(p->ch) < sz) break;
if (RMNET_IS_MODE_IP(opmode) ? (sz > dev->mtu) :
(sz > (dev->mtu + ETH_HLEN))) {
pr_err("rmnet_recv() discarding %d len (%d mtu)\n",
sz, RMNET_IS_MODE_IP(opmode) ?
dev->mtu : (dev->mtu + ETH_HLEN));
ptr = 0;
} else {
skb = dev_alloc_skb(sz + NET_IP_ALIGN);
if (skb == NULL) {
pr_err("rmnet_recv() cannot allocate skb\n");
} else {
skb->dev = dev;
skb_reserve(skb, NET_IP_ALIGN);
ptr = skb_put(skb, sz);
wake_lock_timeout(&p->wake_lock, HZ / 2);
if (smd_read(p->ch, ptr, sz) != sz) {
pr_err("rmnet_recv() smd lied about avail?!");
ptr = 0;
dev_kfree_skb_irq(skb);
} else {
/* Handle Rx frame format */
spin_lock_irqsave(&p->lock, flags);
opmode = p->operation_mode;
spin_unlock_irqrestore(&p->lock, flags);
if (RMNET_IS_MODE_IP(opmode)) {
/* Driver in IP mode */
skb->protocol =
rmnet_ip_type_trans(skb, dev);
} else {
/* Driver in Ethernet mode */
skb->protocol =
eth_type_trans(skb, dev);
}
if (RMNET_IS_MODE_IP(opmode) ||
count_this_packet(ptr, skb->len)) {
#if 0
p->wakeups_rcv +=
rmnet_cause_wakeup(p);
#endif
p->stats.rx_packets++;
p->stats.rx_bytes += skb->len;
}
netif_rx(skb);
}
continue;
}
}
if (smd_read(p->ch, ptr, sz) != sz)
pr_err("rmnet_recv() smd lied about avail?!");
}
}
static void diag_smd_cntl_send_req(int proc_num)
{
int data_len = 0, type = -1, count_bytes = 0, j, r;
struct bindpkt_params_per_process *pkt_params =
kzalloc(sizeof(struct bindpkt_params_per_process), GFP_KERNEL);
struct diag_ctrl_msg *msg;
struct cmd_code_range *range;
struct bindpkt_params *temp;
void *buf = NULL;
smd_channel_t *smd_ch = NULL;
if (proc_num == MODEM_PROC) {
buf = driver->buf_in_cntl;
smd_ch = driver->ch_cntl;
} else if (proc_num == QDSP_PROC) {
buf = driver->buf_in_qdsp_cntl;
smd_ch = driver->chqdsp_cntl;
} else if (proc_num == WCNSS_PROC) {
buf = driver->buf_in_wcnss_cntl;
smd_ch = driver->ch_wcnss_cntl;
}
if (!smd_ch || !buf)
return;
r = smd_read_avail(smd_ch);
if (r > IN_BUF_SIZE) {
if (r < MAX_IN_BUF_SIZE) {
pr_err("diag: SMD CNTL sending pkt upto %d bytes", r);
buf = krealloc(buf, r, GFP_KERNEL);
} else {
pr_err("diag: CNTL pkt > %d bytes", MAX_IN_BUF_SIZE);
kfree(pkt_params);
return;
}
}
if (buf && r > 0) {
smd_read(smd_ch, buf, r);
while (count_bytes + HDR_SIZ <= r) {
type = *(uint32_t *)(buf);
data_len = *(uint32_t *)(buf + 4);
count_bytes = count_bytes+HDR_SIZ+data_len;
if (type == DIAG_CTRL_MSG_REG && r >= count_bytes) {
msg = buf+HDR_SIZ;
range = buf+HDR_SIZ+
sizeof(struct diag_ctrl_msg);
pkt_params->count = msg->count_entries;
temp = kzalloc(pkt_params->count * sizeof(struct
bindpkt_params), GFP_KERNEL);
for (j = 0; j < pkt_params->count; j++) {
temp->cmd_code = msg->cmd_code;
temp->subsys_id = msg->subsysid;
if (proc_num == MODEM_PROC)
temp->client_id =
(uint32_t)(driver->ch);
else if (proc_num == QDSP_PROC)
temp->client_id =
(uint32_t)(driver->chqdsp);
else if (proc_num == WCNSS_PROC)
temp->client_id =
(uint32_t)(driver->ch_wcnss);
temp->proc_id = proc_num;
temp->cmd_code_lo = range->cmd_code_lo;
temp->cmd_code_hi = range->cmd_code_hi;
range++;
temp++;
}
temp -= pkt_params->count;
pkt_params->params = temp;
diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
(unsigned long)pkt_params);
kfree(temp);
buf = buf + HDR_SIZ + data_len;
}
}
}
kfree(pkt_params);
}
/* Called in soft-irq context */
static void smd_net_data_handler(unsigned long arg)
{
struct net_device *dev = (struct net_device *) arg;
struct rmnet_private *p = netdev_priv(dev);
struct sk_buff *skb;
void *ptr = 0;
int sz;
u32 opmode = p->operation_mode;
unsigned long flags;
for (;;) {
sz = smd_cur_packet_size(p->ch);
if (sz == 0) break;
if (smd_read_avail(p->ch) < sz) break;
skb = dev_alloc_skb(sz + NET_IP_ALIGN);
if (skb == NULL) {
pr_err("[%s] rmnet_recv() cannot allocate skb\n",
dev->name);
/* out of memory, reschedule a later attempt */
smd_net_data_tasklet.data = (unsigned long)dev;
tasklet_schedule(&smd_net_data_tasklet);
break;
} else {
skb->dev = dev;
skb_reserve(skb, NET_IP_ALIGN);
ptr = skb_put(skb, sz);
wake_lock_timeout(&p->wake_lock, HZ / 2);
if (smd_read(p->ch, ptr, sz) != sz) {
pr_err("[%s] rmnet_recv() smd lied about avail?!",
dev->name);
ptr = 0;
dev_kfree_skb_irq(skb);
} else {
/* Handle Rx frame format */
spin_lock_irqsave(&p->lock, flags);
opmode = p->operation_mode;
spin_unlock_irqrestore(&p->lock, flags);
if (RMNET_IS_MODE_IP(opmode)) {
/* Driver in IP mode */
skb->protocol =
rmnet_ip_type_trans(skb, dev);
} else {
/* Driver in Ethernet mode */
skb->protocol =
eth_type_trans(skb, dev);
}
if (RMNET_IS_MODE_IP(opmode) ||
count_this_packet(ptr, skb->len)) {
#ifdef CONFIG_MSM_RMNET_DEBUG
p->wakeups_rcv +=
rmnet_cause_wakeup(p);
#endif
p->stats.rx_packets++;
p->stats.rx_bytes += skb->len;
}
DBG1("[%s] Rx packet #%lu len=%d\n",
dev->name, p->stats.rx_packets,
skb->len);
/* Deliver to network stack */
netif_rx(skb);
}
continue;
}
if (smd_read(p->ch, ptr, sz) != sz)
pr_err("[%s] rmnet_recv() smd lied about avail?!",
dev->name);
}
}
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;
}
