void diag_send_feature_mask_update(struct diag_smd_info *smd_info)
{
void *buf = driver->buf_feature_mask_update;
int header_size = sizeof(struct diag_ctrl_feature_mask);
int wr_size = -ENOMEM, retry_count = 0;
uint8_t feature_bytes[FEATURE_MASK_LEN_BYTES] = {0, 0};
int total_len = 0;
if (!smd_info) {
pr_err("diag: In %s, null smd info pointer\n",
__func__);
return;
}
if (!smd_info->ch) {
pr_err("diag: In %s, smd channel not open for peripheral: %d, type: %d\n",
__func__, smd_info->peripheral, smd_info->type);
return;
}
mutex_lock(&driver->diag_cntl_mutex);
/* send feature mask update */
driver->feature_mask->ctrl_pkt_id = DIAG_CTRL_MSG_FEATURE;
driver->feature_mask->ctrl_pkt_data_len = 4 + FEATURE_MASK_LEN_BYTES;
driver->feature_mask->feature_mask_len = FEATURE_MASK_LEN_BYTES;
memcpy(buf, driver->feature_mask, header_size);
feature_bytes[0] |= F_DIAG_INT_FEATURE_MASK;
feature_bytes[0] |= F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
feature_bytes[0] |= driver->supports_separate_cmdrsp ?
F_DIAG_REQ_RSP_CHANNEL : 0;
feature_bytes[0] |= driver->supports_apps_hdlc_encoding ?
F_DIAG_HDLC_ENCODE_IN_APPS_MASK : 0;
feature_bytes[1] |= F_DIAG_OVER_STM;
memcpy(buf+header_size, &feature_bytes, FEATURE_MASK_LEN_BYTES);
total_len = header_size + FEATURE_MASK_LEN_BYTES;
while (retry_count < 3) {
mutex_lock(&smd_info->smd_ch_mutex);
wr_size = smd_write(smd_info->ch, buf, total_len);
mutex_unlock(&smd_info->smd_ch_mutex);
if (wr_size == -ENOMEM) {
retry_count++;
/*
* The smd channel is full. Delay while
* smd processes existing data and smd
* has memory become available. The delay
* of 10000 was determined empirically as
* best value to use.
*/
usleep_range(10000, 10100);
} else
break;
}
if (wr_size != total_len)
pr_err("diag: In %s, peripheral %d fail feature update, size: %d, tried: %d",
__func__, smd_info->peripheral, wr_size, total_len);
mutex_unlock(&driver->diag_cntl_mutex);
}
static int qmi_send(struct qmi_ctxt *ctxt, struct qmi_msg *msg)
{
unsigned char *data;
unsigned hlen;
unsigned len;
int r;
// qmi_dump_msg(msg, "send");
if (msg->service == QMI_CTL) {
hlen = QMUX_HEADER - 1;
} else {
hlen = QMUX_HEADER;
}
/* QMUX length is total header + total payload - IFC selector */
len = hlen + msg->size - 1;
if (len > 0xffff)
return -1;
data = msg->tlv - hlen;
/* prepend encap and qmux header */
*data++ = 0x01; /* ifc selector */
/* qmux header */
*data++ = len;
*data++ = len >> 8;
*data++ = 0x00; /* flags: client */
*data++ = msg->service;
*data++ = msg->client_id;
/* qmi header */
*data++ = 0x00; /* flags: send */
*data++ = msg->txn_id;
if (msg->service != QMI_CTL)
*data++ = msg->txn_id >> 8;
*data++ = msg->type;
*data++ = msg->type >> 8;
*data++ = msg->size;
*data++ = msg->size >> 8;
// add channel number here
*(uint32_t*)(msg->tlv + msg->size) = ctxt->ch_num;
DBG("send %d %d\n", len + 1 + 4, ctxt->ch_num);
// print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, msg->tlv - hlen, len + 1 + 4);
/* len + 1 takes the interface selector into account */
// and add ch_num_size
r = smd_write(ctrl_ch, msg->tlv - hlen, len + 1 + 4);
if (r != len) {
return -1;
} else {
return 0;
}
}
void diag_send_msg_mask_update(smd_channel_t *ch, int updated_ssid_first,
int updated_ssid_last, int proc)
{
void *buf = driver->buf_msg_mask_update;
int first, last, actual_last, size = -ENOMEM, retry_count = 0;
int header_size = sizeof(struct diag_ctrl_msg_mask);
uint8_t *ptr = driver->msg_masks;
mutex_lock(&driver->diag_cntl_mutex);
while (*(uint32_t *)(ptr + 4)) {
first = *(uint32_t *)ptr;
ptr += 4;
last = *(uint32_t *)ptr;
ptr += 4;
actual_last = *(uint32_t *)ptr;
ptr += 4;
if ((updated_ssid_first >= first && updated_ssid_last <=
actual_last) || (updated_ssid_first == ALL_SSID)) {
/* send f3 mask update */
driver->msg_mask->cmd_type = DIAG_CTRL_MSG_F3_MASK;
driver->msg_mask->msg_mask_size = actual_last -
first + 1;
driver->msg_mask->data_len = 11 +
4 * (driver->msg_mask->msg_mask_size);
driver->msg_mask->stream_id = 1; /* 2, if dual stream */
driver->msg_mask->status = 3; /* status valid mask */
driver->msg_mask->msg_mode = 0; /* Legcay mode */
driver->msg_mask->ssid_first = first;
driver->msg_mask->ssid_last = actual_last;
memcpy(buf, driver->msg_mask, header_size);
memcpy(buf+header_size, ptr,
4 * (driver->msg_mask->msg_mask_size));
if (ch) {
while (retry_count < 3) {
size = smd_write(ch, buf, header_size +
4*(driver->msg_mask->msg_mask_size));
if (size == -ENOMEM) {
retry_count++;
WAIT_FOR_SMD(5, 2000);
} else
break;
}
if (size != header_size +
4*(driver->msg_mask->msg_mask_size))
pr_err("diag: proc %d, msg mask update fail %d, tried %d\n",
proc, size, (header_size +
4*(driver->msg_mask->msg_mask_size)));
else
pr_debug("diag: sending mask update for ssid first %d, last %d on PROC %d\n",
first, actual_last, proc);
} else
pr_err("diag: proc %d, ch invalid msg mask update\n",
proc);
}
ptr += MAX_SSID_PER_RANGE*4;
}
mutex_unlock(&driver->diag_cntl_mutex);
}
static int hci_smd_send_frame(struct sk_buff *skb)
{
int len;
int avail;
int ret = 0;
wake_lock(&hs.wake_lock_tx);
switch (bt_cb(skb)->pkt_type) {
case HCI_COMMAND_PKT:
avail = smd_write_avail(hs.event_channel);
if (!avail) {
BT_ERR("No space available for smd frame");
ret = -ENOSPC;
}
len = smd_write(hs.event_channel, skb->data, skb->len);
if (len < skb->len) {
BT_ERR("Failed to write Command %d", len);
ret = -ENODEV;
}
break;
case HCI_ACLDATA_PKT:
case HCI_SCODATA_PKT:
avail = smd_write_avail(hs.data_channel);
if (!avail) {
BT_ERR("No space available for smd frame");
ret = -ENOSPC;
}
len = smd_write(hs.data_channel, skb->data, skb->len);
if (len < skb->len) {
BT_ERR("Failed to write Data %d", len);
ret = -ENODEV;
}
break;
default:
BT_ERR("Uknown packet type");
ret = -ENODEV;
break;
}
kfree_skb(skb);
wake_unlock(&hs.wake_lock_tx);
return ret;
}
void diag_send_diag_mode_update_by_smd(struct diag_smd_info *smd_info,
int real_time)
{
struct diag_ctrl_msg_diagmode diagmode;
char buf[sizeof(struct diag_ctrl_msg_diagmode)];
int msg_size = sizeof(struct diag_ctrl_msg_diagmode);
int wr_size = -ENOMEM, retry_count = 0, timer;
/* For now only allow the modem to receive the message */
if (!smd_info || smd_info->type != SMD_CNTL_TYPE)
return;
mutex_lock(&driver->diag_cntl_mutex);
diagmode.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE;
diagmode.ctrl_pkt_data_len = 36;
diagmode.version = 1;
diagmode.sleep_vote = real_time ? 1 : 0;
/*
* 0 - Disables real-time logging (to prevent
* frequent APPS wake-ups, etc.).
* 1 - Enable real-time logging
*/
diagmode.real_time = real_time;
diagmode.use_nrt_values = 0;
diagmode.commit_threshold = 0;
diagmode.sleep_threshold = 0;
diagmode.sleep_time = 0;
diagmode.drain_timer_val = 0;
diagmode.event_stale_timer_val = 0;
memcpy(buf, &diagmode, msg_size);
if (smd_info->ch) {
while (retry_count < 3) {
wr_size = smd_write(smd_info->ch, buf, msg_size);
if (wr_size == -ENOMEM) {
retry_count++;
for (timer = 0; timer < 5; timer++)
udelay(2000);
} else {
driver->real_time_mode = real_time;
break;
}
}
if (wr_size != msg_size)
pr_err("diag: proc %d fail feature update %d, tried %d",
smd_info->peripheral,
wr_size, msg_size);
} else {
pr_err("diag: ch invalid, feature update on proc %d\n",
smd_info->peripheral);
}
mutex_unlock(&driver->diag_cntl_mutex);
}
static int radio_hci_smd_send_frame(struct sk_buff *skb)
{
int len = 0;
len = smd_write(hs.fm_channel, skb->data, skb->len);
if (len < skb->len) {
FMDERR("Failed to write Data %d", len);
return -ENODEV;
}
return 0;
}
static int smd_tty_write(struct tty_struct *tty, const unsigned char *buf, int len)
{
struct smd_tty_info *info = tty->driver_data;
int avail;
avail = smd_write_avail(info->ch);
if (len > avail)
len = avail;
return smd_write(info->ch, buf, len);
}
static ssize_t smd_lge_write(struct file *fp, const char __user *buf,
size_t count, loff_t *pos)
{
int len = 0, ch_avail = 0, ret = 0;
SMD_LGE_DBG("%s \n", __func__);
if (psmd_device->ch == 0) {
SMD_LGE_INFO("%s : psmd_device->ch is NULL \n", __func__);
return -EFAULT;
}
len = count ;
if (copy_from_user(psmd_device->tx_buff, buf, count))
return -EFAULT;
SMD_LGE_DBG("%s : received len ( %d bytes ) from user \n",
__func__, count);
while (len > 0) {
ch_avail = smd_write_avail(psmd_device->ch);
SMD_LGE_DBG("%s : ch_avail = %d bytes, len = %d bytes \n",
__func__, ch_avail, len);
if (ch_avail < len) {
ret = smd_write(psmd_device->ch, psmd_device->tx_buff,
ch_avail);
len -= ch_avail;
} else {
ret = smd_write(psmd_device->ch, psmd_device->tx_buff,
len);
len -= len;
}
}
SMD_LGE_DBG("%s : write return value = %d \n", __func__, ret);
return ret;
}
/*
* Sends a message to the ADSP via SMD.
*
* @param hdr Specifies message type and other meta data
* @param msg_ptr Pointer to the message contents.
* Must be freed within this function if no error is returned.
*
* @return 0 upon success; < 0 upon error
*/
static int
sns_ocmem_send_msg(struct sns_ocmem_hdr_s *hdr, void const *msg_ptr)
{
int rv = 0;
int err = 0;
void *temp = NULL;
int size = 0;
if (hdr == NULL) {
pr_err("%s: NULL message header\n", __func__);
rv = -EINVAL;
goto out;
}
size = sizeof(struct sns_ocmem_hdr_s) + hdr->msg_size;
temp = kzalloc(sizeof(struct sns_ocmem_hdr_s) + hdr->msg_size,
GFP_KERNEL);
if (temp == NULL) {
pr_err("%s: allocation failure\n", __func__);
rv = -ENOMEM;
goto out;
}
hdr->dst_module = SNS_OCMEM_MODULE_ADSP;
hdr->src_module = SNS_OCMEM_MODULE_KERNEL;
memcpy(temp, hdr, sizeof(struct sns_ocmem_hdr_s));
memcpy((char *)temp + sizeof(struct sns_ocmem_hdr_s),
msg_ptr, hdr->msg_size);
pr_debug("%s: send msg type: %i size: %i id: %i dst: %i src: %i\n",
__func__, hdr->msg_type, hdr->msg_size,
hdr->msg_id, hdr->dst_module, hdr->src_module);
if (sns_ctl.smd_ch == NULL) {
pr_err("%s: null smd_ch\n", __func__);
rv = -EINVAL;
}
err = smd_write(sns_ctl.smd_ch, temp, size);
if (err < 0) {
pr_err("%s: smd_write failed %i\n", __func__, err);
rv = -ECOMM;
} else {
pr_debug("%s smd_write successful ret=%d\n",
__func__, err);
}
kfree(temp);
out:
return rv;
}
static int qmi_send(struct qmi_ctxt *ctxt, struct qmi_msg *msg)
{
unsigned char *data;
unsigned hlen;
unsigned len;
int r;
qmi_dump_msg(msg, "send");
if (msg->service == QMI_CTL) {
hlen = QMUX_HEADER - 1;
} else {
hlen = QMUX_HEADER;
}
/* QMUX length is total header + total payload - IFC selector */
len = hlen + msg->size - 1;
if (len > 0xffff)
return -1;
data = msg->tlv - hlen;
/* prepend encap and qmux header */
*data++ = 0x01; /* ifc selector */
/* qmux header */
*data++ = len;
*data++ = len >> 8;
*data++ = 0x00; /* flags: client */
*data++ = msg->service;
*data++ = msg->client_id;
/* qmi header */
*data++ = 0x00; /* flags: send */
*data++ = msg->txn_id;
if (msg->service != QMI_CTL)
*data++ = msg->txn_id >> 8;
*data++ = msg->type;
*data++ = msg->type >> 8;
*data++ = msg->size;
*data++ = msg->size >> 8;
/* len + 1 takes the interface selector into account */
r = smd_write(ctxt->ch, msg->tlv - hlen, len + 1);
if (r != len) {
return -1;
} else {
return 0;
}
}
static int qmi_send(struct qmi_ctxt *ctxt, struct qmi_msg *msg)
{
unsigned char *data;
unsigned hlen;
unsigned len;
int r;
qmi_dump_msg(msg, "send");
if (msg->service == QMI_CTL) {
hlen = QMUX_HEADER - 1;
} else {
hlen = QMUX_HEADER;
}
len = hlen + msg->size - 1;
if (len > 0xffff)
return -1;
data = msg->tlv - hlen;
*data++ = 0x01;
*data++ = len;
*data++ = len >> 8;
*data++ = 0x00;
*data++ = msg->service;
*data++ = msg->client_id;
*data++ = 0x00;
*data++ = msg->txn_id;
if (msg->service != QMI_CTL)
*data++ = msg->txn_id >> 8;
*data++ = msg->type;
*data++ = msg->type >> 8;
*data++ = msg->size;
*data++ = msg->size >> 8;
r = smd_write(ctxt->ch, msg->tlv - hlen, len + 1);
if (r != len) {
return -1;
} else {
return 0;
}
}
int diag_send_stm_state(struct diag_smd_info *smd_info,
uint8_t stm_control_data)
{
struct diag_ctrl_msg_stm stm_msg;
int msg_size = sizeof(struct diag_ctrl_msg_stm);
int retry_count = 0;
int wr_size = 0;
int success = 0;
if (!smd_info || (smd_info->type != SMD_CNTL_TYPE) ||
(driver->peripheral_supports_stm[smd_info->peripheral] ==
DISABLE_STM)) {
return -EINVAL;
}
if (smd_info->ch) {
stm_msg.ctrl_pkt_id = 21;
stm_msg.ctrl_pkt_data_len = 5;
stm_msg.version = 1;
stm_msg.control_data = stm_control_data;
while (retry_count < 3) {
mutex_lock(&smd_info->smd_ch_mutex);
wr_size = smd_write(smd_info->ch, &stm_msg, msg_size);
mutex_unlock(&smd_info->smd_ch_mutex);
if (wr_size == -ENOMEM) {
/*
* The smd channel is full. Delay while
* smd processes existing data and smd
* has memory become available. The delay
* of 10000 was determined empirically as
* best value to use.
*/
retry_count++;
usleep_range(10000, 10000);
} else {
success = 1;
break;
}
}
if (wr_size != msg_size) {
pr_err("diag: In %s, proc %d fail STM update %d, tried %d",
__func__, smd_info->peripheral, wr_size,
msg_size);
success = 0;
}
} else {
pr_err("diag: In %s, ch invalid, STM update on proc %d\n",
__func__, smd_info->peripheral);
}
return success;
}
void diag_send_log_mask_update(smd_channel_t *ch, int equip_id)
{
void *buf = driver->buf_log_mask_update;
int header_size = sizeof(struct diag_ctrl_log_mask);
struct mask_info *ptr = (struct mask_info *)driver->log_masks;
int i, size, wr_size = -ENOMEM, retry_count = 0;
mutex_lock(&driver->diag_cntl_mutex);
for (i = 0; i < MAX_EQUIP_ID; i++) {
size = (ptr->num_items+7)/8;
/* reached null entry */
if ((ptr->equip_id == 0) && (ptr->index == 0))
break;
driver->log_mask->cmd_type = DIAG_CTRL_MSG_LOG_MASK;
driver->log_mask->num_items = ptr->num_items;
driver->log_mask->data_len = 11 + size;
driver->log_mask->stream_id = 1; /* 2, if dual stream */
driver->log_mask->status = 3; /* status for valid mask */
driver->log_mask->equip_id = ptr->equip_id;
driver->log_mask->log_mask_size = size;
/* send only desired update, NOT ALL */
if (equip_id == ALL_EQUIP_ID || equip_id ==
driver->log_mask->equip_id) {
memcpy(buf, driver->log_mask, header_size);
memcpy(buf+header_size, driver->log_masks+ptr->index,
size);
if (ch) {
while (retry_count < 3) {
wr_size = smd_write(ch, buf,
header_size + size);
if (wr_size == -ENOMEM) {
retry_count++;
WAIT_FOR_SMD(5, 2000);
} else
break;
}
if (wr_size != header_size + size)
pr_err("diag: log mask update failed %d, tried %d",
wr_size, header_size + size);
else
pr_debug("diag: updated log equip ID %d,len %d\n",
driver->log_mask->equip_id,
driver->log_mask->log_mask_size);
} else
pr_err("diag: ch not valid for log update\n");
}
ptr++;
}
mutex_unlock(&driver->diag_cntl_mutex);
}
static int smd_tty_write(struct tty_struct *tty, const unsigned char *buf, int len)
{
struct smd_tty_info *info = tty->driver_data;
int avail;
/* if we're writing to a packet channel we will
** never be able to write more data than there
** is currently space for
*/
avail = smd_write_avail(info->ch);
if (len > avail)
len = avail;
return smd_write(info->ch, buf, len);
}
void diag_process_hdlc(void *data, unsigned len)
{
struct diag_hdlc_decode_type hdlc;
int ret, type = 0;
#ifdef DIAG_DEBUG
int i;
printk(KERN_INFO "\n HDLC decode function, len of data %d\n", len);
#endif
hdlc.dest_ptr = driver->hdlc_buf;
hdlc.dest_size = USB_MAX_OUT_BUF;
hdlc.src_ptr = data;
hdlc.src_size = len;
hdlc.src_idx = 0;
hdlc.dest_idx = 0;
hdlc.escaping = 0;
ret = diag_hdlc_decode(&hdlc);
if (ret)
type = diag_process_apps_pkt(driver->hdlc_buf,
hdlc.dest_idx - 3);
else if (driver->debug_flag) {
printk(KERN_ERR "Packet dropped due to bad HDLC coding/CRC"
" errors or partial packet received, packet"
" length = %d\n", len);
print_hex_dump(KERN_DEBUG, "Dropped Packet Data: ", 16, 1,
DUMP_PREFIX_ADDRESS, data, len, 1);
driver->debug_flag = 0;
}
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n hdlc.dest_idx = %d \n", hdlc.dest_idx);
for (i = 0; i < hdlc.dest_idx; i++)
printk(KERN_DEBUG "\t%x", *(((unsigned char *)
driver->hdlc_buf)+i));
#endif
/* ignore 2 bytes for CRC, one for 7E and send */
if ((driver->ch) && (ret) && (type) && (hdlc.dest_idx > 3)) {
APPEND_DEBUG('g');
smd_write(driver->ch, driver->hdlc_buf, hdlc.dest_idx - 3);
APPEND_DEBUG('h');
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to SMD, pkt length %d \n", len);
print_hex_dump(KERN_DEBUG, "Written Packet Data to SMD: ", 16,
1, DUMP_PREFIX_ADDRESS, data, len, 1);
#endif
}
}
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 int wcnss_smd_tx(void *data, int len)
{
int ret = 0;
ret = smd_write_avail(penv->smd_ch);
if (ret < len) {
pr_err("wcnss: no space available for smd frame\n");
return -ENOSPC;
}
ret = smd_write(penv->smd_ch, data, len);
if (ret < len) {
pr_err("wcnss: failed to write Command %d", len);
ret = -ENODEV;
}
return ret;
}
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] %s: smd_write returned error %d",
dev->name, __func__, smd_ret);
p->stats.tx_errors++;
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
}
DBG1("[%s] Tx packet #%lu len=%d mark=0x%x\n",
dev->name, p->stats.tx_packets, skb->len, skb->mark);
xmit_out:
/* data xmited, safe to release skb */
dev_kfree_skb_irq(skb);
return 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;
if (netif_queue_stopped(dev)) {
pr_err("fatal: rmnet_xmit called when netif_queue is stopped");
return 0;
}
if (smd_write_avail(ch) < skb->len) {
rmnet_stop(dev);
/* schedule a function to poll at exponential interval */
init_timer(&p->smd_poll_timer);
p->smd_poll_timer.expires = jiffies
+ ((SMD_POLL_MILLISECS*HZ)/1000);
p->smd_poll_timer.function = rmnet_poll_smd_write;
if (p->skb)
pr_err("fatal: p->skb was non-zero when"
"we tried to scheduled timer");
p->skb = skb;
p->smd_poll_timer.data = (unsigned long)dev;
add_timer(&p->smd_poll_timer);
} else {
smd_ret = smd_write(ch, skb->data, skb->len);
if (smd_ret != skb->len) {
pr_err("fatal: smd_write returned error %d", smd_ret);
return 0;
}
if (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
}
/* data xmited, safe to release skb */
dev_kfree_skb_irq(skb);
}
return 0;
}
static int smd_tty_write(struct tty_struct *tty, const unsigned char *buf, int len)
{
struct smd_tty_info *info = tty->driver_data;
int avail;
if (is_in_reset(info))
return -ENETRESET;
avail = smd_write_avail(info->ch);
if (!avail) {
smd_enable_read_intr(info->ch);
return 0;
}
if (len > avail)
len = avail;
return smd_write(info->ch, buf, len);
}
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;
}
int __apr_tal_write(struct apr_svc_ch_dev *apr_ch, void *data, int len)
{
int w_len;
unsigned long flags;
spin_lock_irqsave(&apr_ch->w_lock, flags);
if (smd_write_avail(apr_ch->ch) < len) {
spin_unlock_irqrestore(&apr_ch->w_lock, flags);
return -EAGAIN;
}
w_len = smd_write(apr_ch->ch, data, len);
spin_unlock_irqrestore(&apr_ch->w_lock, flags);
pr_debug("apr_tal:w_len = %d\n", w_len);
if (w_len != len) {
pr_err("apr_tal: Error in write\n");
return -ENETRESET;
}
return w_len;
}
static void diag_process_hdlc(struct diag_context *ctxt, void *_data, unsigned len)
{
unsigned char *data = _data;
unsigned count = ctxt->hdlc_count;
unsigned escape = ctxt->hdlc_escape;
unsigned char *hdlc = ctxt->hdlc_buf;
while (len-- > 0) {
unsigned char x = *data++;
if (x == 0x7E) {
if (count > 2) {
/* we're just ignoring the crc here */
//TRACE("PC>", hdlc, count - 2, 0);
if (ctxt->ch) {
smd_write(ctxt->ch, hdlc, count - 2);
smd_xfer_count_func(count - 2, data_set_tx);
}
}
count = 0;
escape = 0;
} else if (x == 0x7D) {
escape = 1;
} else {
if (escape) {
x = x ^ 0x20;
escape = 0;
}
hdlc[count++] = x;
/* discard frame if we overflow */
if (count == HDLC_MAX)
count = 0;
}
}
ctxt->hdlc_count = count;
ctxt->hdlc_escape = escape;
}
void diag_send_event_mask_update(smd_channel_t *ch, int num_bytes)
{
void *buf = driver->buf_event_mask_update;
int header_size = sizeof(struct diag_ctrl_event_mask);
int wr_size = -ENOMEM, retry_count = 0;
mutex_lock(&driver->diag_cntl_mutex);
if (num_bytes == 0) {
pr_debug("diag: event mask not set yet, so no update\n");
mutex_unlock(&driver->diag_cntl_mutex);
return;
}
/* send event mask update */
driver->event_mask->cmd_type = DIAG_CTRL_MSG_EVENT_MASK;
driver->event_mask->data_len = 7 + num_bytes;
driver->event_mask->stream_id = 1; /* 2, if dual stream */
driver->event_mask->status = 3; /* status for valid mask */
driver->event_mask->event_config = diag_event_config; /* event config */
driver->event_mask->event_mask_size = num_bytes;
memcpy(buf, driver->event_mask, header_size);
memcpy(buf+header_size, driver->event_masks, num_bytes);
if (ch) {
while (retry_count < 3) {
wr_size = smd_write(ch, buf, header_size + num_bytes);
if (wr_size == -ENOMEM) {
retry_count++;
WAIT_FOR_SMD(5, 2000);
} else
break;
}
if (wr_size != header_size + num_bytes)
pr_err("diag: error writing event mask %d, tried %d\n",
wr_size, header_size + num_bytes);
} else
pr_err("diag: ch not valid for event update\n");
mutex_unlock(&driver->diag_cntl_mutex);
}
static void diag_send_ctrl_msg(struct diag_smd_info *smd_info,
const void *msg,
int len)
{
int wr_size = -ENOMEM, retry_count = 0, timer;
if (smd_info->ch) {
while (retry_count < 3) {
wr_size = smd_write(smd_info->ch, msg, len);
if (wr_size == -ENOMEM) {
retry_count++;
for (timer = 0; timer < 5; timer++)
usleep_range(2000, 3000);
} else
break;
}
if (wr_size != len)
pr_err("diag: proc %d failed sending msg %d, tried %d",
smd_info->peripheral,
wr_size, len);
} else {
pr_err("diag: ch invalid on proc %d\n", smd_info->peripheral);
}
}
static int update_modem(enum ssm_ipc_req ipc_req, struct ssm_driver *ssm,
int length, char *data)
{
unsigned int packet_len = length + SSM_MSG_FIELD_LEN;
int rc = 0, count;
snprintf(ssm->smd_buffer, SSM_MSG_FIELD_LEN + 1, "%10u|", ipc_req);
memcpy(ssm->smd_buffer + SSM_MSG_FIELD_LEN, data, length);
if (smd_write_avail(ssm->ch) < packet_len) {
dev_err(ssm->dev, "Not enough space dropping request\n");
rc = -ENOSPC;
goto out;
}
count = smd_write(ssm->ch, ssm->smd_buffer, packet_len);
if (count < packet_len) {
dev_err(ssm->dev, "smd_write failed for %d\n", ipc_req);
rc = -EIO;
}
out:
return rc;
}
void diag_process_hdlc(void *data, unsigned len)
{
struct diag_hdlc_decode_type hdlc;
int ret, type = 0;
#if HPST_FUN
unsigned char *buf_9k = NULL;
int path;
#endif
#ifdef DIAG_DEBUG
int i;
DIAGFWD_INFO("\n HDLC decode function, len of data %d\n", len);
#endif
hdlc.dest_ptr = driver->hdlc_buf;
hdlc.dest_size = USB_MAX_OUT_BUF;
hdlc.src_ptr = data;
hdlc.src_size = len;
hdlc.src_idx = 0;
hdlc.dest_idx = 0;
hdlc.escaping = 0;
ret = diag_hdlc_decode(&hdlc);
if (ret)
type = diag_process_apps_pkt(driver->hdlc_buf,
hdlc.dest_idx - 3);
else if (driver->debug_flag) {
DIAGFWD_INFO("Packet dropped due to bad HDLC coding/CRC"
" errors or partial packet received, packet"
" length = %d\n", len);
print_hex_dump(KERN_DEBUG, "Dropped Packet Data: ", DUMP_PREFIX_ADDRESS, 16, 1,
data, len, 1);
driver->debug_flag = 0;
}
#ifdef CONFIG_DIAG_NO_MODEM
if (type == 1) { /* implies this packet is NOT meant for apps */
if (driver->chqdsp)
smd_write(driver->chqdsp, driver->hdlc_buf,
hdlc.dest_idx - 3);
type = 0;
}
#endif /* NO MODEM present */
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n hdlc.dest_idx = %d", hdlc.dest_idx);
for (i = 0; i < hdlc.dest_idx; i++)
printk(KERN_DEBUG "\t%x", *(((unsigned char *)
driver->hdlc_buf)+i));
#endif /* DIAG DEBUG */
/* ignore 2 bytes for CRC, one for 7E and send */
if ((driver->ch) && (ret) && (type) && (hdlc.dest_idx > 3)) {
/* APPEND_DEBUG('g'); */
#if defined(CONFIG_ARCH_MSM8X60_LTE) || defined(CONFIG_ARCH_MSM8X60)
smd_write(driver->ch, driver->hdlc_buf, hdlc.dest_idx - 3);
#else //defined(CONFIG_MACH_MECHA)
smd_write(driver->ch, data, len);
#endif
if (diag7k_debug_mask)
print_hex_dump(KERN_DEBUG, "Written Packet Data to SMD: ", DUMP_PREFIX_ADDRESS, 16,
1, data, len, 1);
/* APPEND_DEBUG('h'); */
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to SMD, pkt length %d \n", len);
print_hex_dump(KERN_DEBUG, "Written Packet Data to SMD: ", DUMP_PREFIX_ADDRESS, 16,
1, data, len, 1);
#endif /* DIAG DEBUG */
}
}
static int rpcrouter_smd_loopback_write(void *data, uint32_t len, uint32_t type)
{
return smd_write(smd_loopback_xprt.channel, data, len);
}
/* 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_process_hdlc(void *data, unsigned len)
{
struct diag_hdlc_decode_type hdlc;
int ret, type = 0;
#ifdef DIAG_DEBUG
int i;
#endif
pr_debug("diag: HDLC decode fn, len of data %d\n", len);
hdlc.dest_ptr = driver->hdlc_buf;
hdlc.dest_size = USB_MAX_OUT_BUF;
hdlc.src_ptr = data;
hdlc.src_size = len;
hdlc.src_idx = 0;
hdlc.dest_idx = 0;
hdlc.escaping = 0;
ret = diag_hdlc_decode(&hdlc);
if (ret)
type = diag_process_apps_pkt(driver->hdlc_buf,
hdlc.dest_idx - 3);
else if (driver->debug_flag) {
printk(KERN_ERR "Packet dropped due to bad HDLC coding/CRC"
" errors or partial packet received, packet"
" length = %d\n", len);
print_hex_dump(KERN_DEBUG, "Dropped Packet Data: ", 16, 1,
DUMP_PREFIX_ADDRESS, data, len, 1);
driver->debug_flag = 0;
}
/* send error responses from APPS for Central Routing */
if (type == 1 && chk_config_get_id() == AO8960_TOOLS_ID) {
diag_send_error_rsp(hdlc.dest_idx);
type = 0;
}
/* implies this packet is NOT meant for apps */
if (!(driver->ch) && type == 1) {
if (chk_config_get_id() == AO8960_TOOLS_ID) {
diag_send_error_rsp(hdlc.dest_idx);
} else { /* APQ 8060, Let Q6 respond */
if (driver->chqdsp)
smd_write(driver->chqdsp, driver->hdlc_buf,
hdlc.dest_idx - 3);
}
type = 0;
}
#ifdef DIAG_DEBUG
pr_debug("diag: hdlc.dest_idx = %d", hdlc.dest_idx);
for (i = 0; i < hdlc.dest_idx; i++)
printk(KERN_DEBUG "\t%x", *(((unsigned char *)
driver->hdlc_buf)+i));
#endif /* DIAG DEBUG */
/* ignore 2 bytes for CRC, one for 7E and send */
if ((driver->ch) && (ret) && (type) && (hdlc.dest_idx > 3)) {
APPEND_DEBUG('g');
#ifdef CONFIG_MODEM_DIAG_MASTER
smd_write(driver->ch, data, len);
#else
smd_write(driver->ch, driver->hdlc_buf, hdlc.dest_idx - 3);
#endif
APPEND_DEBUG('h');
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to SMD, pkt length %d\n", len);
print_hex_dump(KERN_DEBUG, "Written Packet Data to SMD: ", 16,
1, DUMP_PREFIX_ADDRESS, data, len, 1);
#endif /* DIAG DEBUG */
}
}
