C++ smd_write函数代码示例

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

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