int diag_send_peripheral_drain_immediate(struct diag_smd_info *smd_info)
{
int err = 0;
struct diag_ctrl_drain_immediate ctrl_pkt;
if (!smd_info)
return -EIO;
if (!driver->peripheral_buffering_support[smd_info->peripheral]) {
pr_debug("diag: In %s, peripheral %d doesn't support buffering\n",
__func__, smd_info->peripheral);
return -EINVAL;
}
ctrl_pkt.pkt_id = DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM;
/* The length of the ctrl pkt is size of version and stream id */
ctrl_pkt.len = sizeof(uint32_t) + sizeof(uint8_t);
ctrl_pkt.version = 1;
ctrl_pkt.stream_id = 1;
err = diag_smd_write(smd_info, &ctrl_pkt, sizeof(ctrl_pkt));
if (err) {
pr_err("diag: Unable to send drain immediate ctrl packet to peripheral %d, err: %d\n",
smd_info->peripheral, err);
}
return err;
}
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 success = 0;
int err = 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;
err = diag_smd_write(smd_info, &stm_msg, msg_size);
if (err) {
pr_err("diag: In %s, unable to write to smd, peripheral: %d, type: %d, len: %d, err: %d\n",
__func__, smd_info->peripheral, smd_info->type,
msg_size, err);
} else {
success = 1;
}
} else {
pr_err("diag: In %s, ch invalid, STM update on proc %d\n",
__func__, smd_info->peripheral);
}
return success;
}
int diag_send_buffering_tx_mode_pkt(struct diag_smd_info *smd_info,
struct diag_buffering_mode_t *params)
{
int err = 0;
struct diag_ctrl_peripheral_tx_mode ctrl_pkt;
if (!smd_info || !params)
return -EIO;
if (!driver->peripheral_buffering_support[smd_info->peripheral]) {
pr_debug("diag: In %s, peripheral %d doesn't support buffering\n",
__func__, smd_info->peripheral);
return -EINVAL;
}
if (params->peripheral != smd_info->peripheral)
return -EINVAL;
switch (params->mode) {
case DIAG_BUFFERING_MODE_STREAMING:
case DIAG_BUFFERING_MODE_THRESHOLD:
case DIAG_BUFFERING_MODE_CIRCULAR:
break;
default:
pr_err("diag: In %s, invalid tx mode: %d\n", __func__,
params->mode);
return -EINVAL;
}
ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE;
/* Control packet length is size of version, stream_id and tx_mode */
ctrl_pkt.len = sizeof(uint32_t) + (2 * sizeof(uint8_t));
ctrl_pkt.version = 1;
ctrl_pkt.stream_id = 1;
ctrl_pkt.tx_mode = params->mode;
err = diag_smd_write(smd_info, &ctrl_pkt, sizeof(ctrl_pkt));
if (err) {
pr_err("diag: Unable to send tx_mode ctrl packet to peripheral %d, err: %d\n",
smd_info->peripheral, err);
goto fail;
}
driver->buffering_mode[smd_info->peripheral].mode = params->mode;
fail:
return err;
}
static 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);
uint8_t feature_bytes[FEATURE_MASK_LEN] = {0, 0};
struct diag_ctrl_feature_mask feature_mask;
int total_len = 0;
int err = 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 */
feature_mask.ctrl_pkt_id = DIAG_CTRL_MSG_FEATURE;
feature_mask.ctrl_pkt_data_len = sizeof(uint32_t) + FEATURE_MASK_LEN;
feature_mask.feature_mask_len = FEATURE_MASK_LEN;
memcpy(buf, &feature_mask, header_size);
DIAG_SET_FEATURE_MASK(F_DIAG_FEATURE_MASK_SUPPORT);
DIAG_SET_FEATURE_MASK(F_DIAG_LOG_ON_DEMAND_APPS);
DIAG_SET_FEATURE_MASK(F_DIAG_STM);
if (driver->supports_separate_cmdrsp)
DIAG_SET_FEATURE_MASK(F_DIAG_REQ_RSP_SUPPORT);
if (driver->supports_apps_hdlc_encoding)
DIAG_SET_FEATURE_MASK(F_DIAG_APPS_HDLC_ENCODE);
DIAG_SET_FEATURE_MASK(F_DIAG_MASK_CENTRALIZATION);
memcpy(buf + header_size, &feature_bytes, FEATURE_MASK_LEN);
total_len = header_size + FEATURE_MASK_LEN;
err = diag_smd_write(smd_info, buf, total_len);
if (err) {
pr_err("diag: In %s, unable to write to smd, peripheral: %d, type: %d, len: %d, err: %d\n",
__func__, smd_info->peripheral, smd_info->type,
total_len, err);
}
mutex_unlock(&driver->diag_cntl_mutex);
}
int diag_send_buffering_wm_values(struct diag_smd_info *smd_info,
struct diag_buffering_mode_t *params)
{
int err = 0;
struct diag_ctrl_set_wq_val ctrl_pkt;
if (!smd_info || !params)
return -EIO;
if (!driver->peripheral_buffering_support[smd_info->peripheral]) {
pr_debug("diag: In %s, peripheral %d doesn't support buffering\n",
__func__, smd_info->peripheral);
return -EINVAL;
}
if (params->peripheral != smd_info->peripheral)
return -EINVAL;
switch (params->mode) {
case DIAG_BUFFERING_MODE_STREAMING:
case DIAG_BUFFERING_MODE_THRESHOLD:
case DIAG_BUFFERING_MODE_CIRCULAR:
break;
default:
pr_err("diag: In %s, invalid tx mode: %d\n", __func__,
params->mode);
return -EINVAL;
}
ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL;
/* Control packet length is size of version, stream_id and wmq values */
ctrl_pkt.len = sizeof(uint32_t) + (3 * sizeof(uint8_t));
ctrl_pkt.version = 1;
ctrl_pkt.stream_id = 1;
ctrl_pkt.high_wm_val = params->high_wm_val;
ctrl_pkt.low_wm_val = params->low_wm_val;
err = diag_smd_write(smd_info, &ctrl_pkt, sizeof(ctrl_pkt));
if (err) {
pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n",
smd_info->peripheral, err);
}
return err;
}
void diag_send_diag_mode_update_by_smd(struct diag_smd_info *smd_info,
int real_time)
{
char buf[sizeof(struct diag_ctrl_msg_diagmode)];
int msg_size = sizeof(struct diag_ctrl_msg_diagmode);
struct diag_smd_info *data = NULL;
int err = 0;
if (!smd_info || smd_info->type != SMD_CNTL_TYPE) {
pr_err("diag: In %s, invalid channel info, smd_info: %p type: %d\n",
__func__, smd_info,
((smd_info) ? smd_info->type : -1));
return;
}
if (smd_info->peripheral < MODEM_DATA ||
smd_info->peripheral > WCNSS_DATA) {
pr_err("diag: In %s, invalid peripheral %d\n", __func__,
smd_info->peripheral);
return;
}
data = &driver->smd_data[smd_info->peripheral];
if (!data)
return;
diag_create_diag_mode_ctrl_pkt(buf, real_time);
mutex_lock(&driver->diag_cntl_mutex);
err = diag_smd_write(smd_info, buf, msg_size);
if (err) {
pr_err("diag: In %s, unable to write to smd, peripheral: %d, type: %d, len: %d, err: %d\n",
__func__, smd_info->peripheral, smd_info->type,
msg_size, err);
} else {
driver->real_time_mode[DIAG_LOCAL_PROC] = real_time;
}
mutex_unlock(&driver->diag_cntl_mutex);
}
int diag_send_diag_mode_update_by_smd(struct diag_smd_info *smd_info,
int real_time)
{
char buf[sizeof(struct diag_ctrl_msg_diagmode)];
int msg_size = sizeof(struct diag_ctrl_msg_diagmode);
int err = 0;
if (!smd_info || smd_info->type != SMD_CNTL_TYPE) {
pr_err("diag: In %s, invalid channel info, smd_info: %pK type: %d\n",
__func__, smd_info,
((smd_info) ? smd_info->type : -1));
return -EIO;
}
if (real_time != MODE_NONREALTIME && real_time != MODE_REALTIME) {
pr_err("diag: In %s, invalid real time mode %d, peripheral: %d\n",
__func__, real_time, smd_info->peripheral);
return -EINVAL;
}
diag_create_diag_mode_ctrl_pkt(buf, real_time);
mutex_lock(&driver->diag_cntl_mutex);
err = diag_smd_write(smd_info, buf, msg_size);
if (err) {
pr_err("diag: In %s, unable to write to smd, peripheral: %d, type: %d, len: %d, err: %d\n",
__func__, smd_info->peripheral, smd_info->type,
msg_size, err);
} else {
driver->real_time_mode[DIAG_LOCAL_PROC] = real_time;
}
mutex_unlock(&driver->diag_cntl_mutex);
return err;
}
static void diag_send_log_mask_update(struct diag_smd_info *smd_info,
int equip_id)
{
int i;
int err = 0;
int send_once = 0;
int header_len = sizeof(struct diag_ctrl_log_mask);
uint8_t *buf = log_mask.update_buf;
uint8_t *temp = NULL;
uint32_t mask_size = 0;
struct diag_ctrl_log_mask ctrl_pkt;
struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;
if (!smd_info)
return;
if (!smd_info->ch) {
pr_debug("diag: In %s, SMD channel is closed, peripheral: %d\n",
__func__, smd_info->peripheral);
return;
}
switch (log_mask.status) {
case DIAG_CTRL_MASK_ALL_DISABLED:
ctrl_pkt.equip_id = 0;
ctrl_pkt.num_items = 0;
ctrl_pkt.log_mask_size = 0;
send_once = 1;
break;
case DIAG_CTRL_MASK_ALL_ENABLED:
ctrl_pkt.equip_id = 0;
ctrl_pkt.num_items = 0;
ctrl_pkt.log_mask_size = 0;
send_once = 1;
break;
case DIAG_CTRL_MASK_VALID:
send_once = 0;
break;
default:
pr_debug("diag: In %s, invalid log_mask status\n", __func__);
return;
}
mutex_lock(&log_mask.lock);
for (i = 0; i < MAX_EQUIP_ID; i++, mask++) {
if (equip_id != i && equip_id != ALL_EQUIP_ID)
continue;
ctrl_pkt.cmd_type = DIAG_CTRL_MSG_LOG_MASK;
ctrl_pkt.stream_id = 1;
ctrl_pkt.status = log_mask.status;
if (log_mask.status == DIAG_CTRL_MASK_VALID) {
mask_size = LOG_ITEMS_TO_SIZE(mask->num_items_tools);
ctrl_pkt.equip_id = i;
ctrl_pkt.num_items = mask->num_items_tools;
ctrl_pkt.log_mask_size = mask_size;
}
ctrl_pkt.data_len = LOG_MASK_CTRL_HEADER_LEN + mask_size;
if (header_len + mask_size > log_mask.update_buf_len) {
temp = krealloc(buf, header_len + mask_size,
GFP_KERNEL);
if (!temp) {
pr_err("diag: Unable to realloc log update buffer, new size: %d, equip_id: %d\n",
header_len + mask_size, equip_id);
break;
}
log_mask.update_buf = temp;
log_mask.update_buf_len = header_len + mask_size;
}
memcpy(buf, &ctrl_pkt, header_len);
if (mask_size > 0)
memcpy(buf + header_len, mask->ptr, mask_size);
err = diag_smd_write(smd_info, buf, header_len + mask_size);
if (err) {
pr_err("diag: Unable to send log masks to peripheral %d, equip_id: %d, err: %d\n",
smd_info->peripheral, i, err);
}
if (send_once || equip_id != ALL_EQUIP_ID)
break;
}
mutex_unlock(&log_mask.lock);
}
static void diag_send_msg_mask_update(struct diag_smd_info *smd_info,
int first, int last)
{
int i;
int err = 0;
int header_len = sizeof(struct diag_ctrl_msg_mask);
int temp_len = 0;
uint8_t *buf = msg_mask.update_buf;
uint8_t *temp = NULL;
uint32_t mask_size = 0;
struct diag_msg_mask_t *mask = (struct diag_msg_mask_t *)msg_mask.ptr;
struct diag_ctrl_msg_mask header;
if (!smd_info)
return;
if (!smd_info->ch) {
pr_debug("diag: In %s, SMD channel is closed, peripheral: %d\n",
__func__, smd_info->peripheral);
return;
}
mutex_lock(&msg_mask.lock);
switch (msg_mask.status) {
case DIAG_CTRL_MASK_ALL_DISABLED:
mask_size = 0;
break;
case DIAG_CTRL_MASK_ALL_ENABLED:
mask_size = 1;
break;
case DIAG_CTRL_MASK_VALID:
break;
default:
pr_debug("diag: In %s, invalid status: %d\n", __func__,
msg_mask.status);
goto err;
}
for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
if (((first < mask->ssid_first) ||
(last > mask->ssid_last_tools)) && first != ALL_SSID) {
continue;
}
if (msg_mask.status == DIAG_CTRL_MASK_VALID) {
mask_size =
mask->ssid_last_tools - mask->ssid_first + 1;
temp_len = mask_size * sizeof(uint32_t);
if (temp_len + header_len <= msg_mask.update_buf_len)
goto proceed;
temp = krealloc(msg_mask.update_buf, temp_len,
GFP_KERNEL);
if (!temp) {
pr_err("diag: In %s, unable to realloc msg_mask update buffer\n",
__func__);
mask_size = (msg_mask.update_buf_len -
header_len) / sizeof(uint32_t);
} else {
msg_mask.update_buf = temp;
msg_mask.update_buf_len = temp_len;
pr_debug("diag: In %s, successfully reallocated msg_mask update buffer to len: %d\n",
__func__, msg_mask.update_buf_len);
}
} else if (msg_mask.status == DIAG_CTRL_MASK_ALL_ENABLED) {
mask_size = 1;
}
proceed:
header.cmd_type = DIAG_CTRL_MSG_F3_MASK;
header.status = msg_mask.status;
header.stream_id = 1;
header.msg_mode = 0;
header.ssid_first = mask->ssid_first;
header.ssid_last = mask->ssid_last_tools;
header.msg_mask_size = mask_size;
mask_size *= sizeof(uint32_t);
header.data_len = MSG_MASK_CTRL_HEADER_LEN + mask_size;
memcpy(buf, &header, header_len);
if (mask_size > 0)
memcpy(buf + header_len, mask->ptr, mask_size);
err = diag_smd_write(smd_info, buf, header_len + mask_size);
if (err) {
pr_err("diag: Unable to send msg masks to peripheral %d\n",
smd_info->peripheral);
}
if (first != ALL_SSID)
break;
}
err:
mutex_unlock(&msg_mask.lock);
}
static void diag_send_event_mask_update(struct diag_smd_info *smd_info)
{
uint8_t *buf = event_mask.update_buf;
uint8_t *temp = NULL;
struct diag_ctrl_event_mask header;
int num_bytes = EVENT_COUNT_TO_BYTES(driver->last_event_id);
int write_len = 0;
int err = 0;
int temp_len = 0;
if (num_bytes <= 0 || num_bytes > driver->event_mask_size) {
pr_debug("diag: In %s, invalid event mask length %d\n",
__func__, num_bytes);
return;
}
if (!smd_info)
return;
if (!smd_info->ch) {
pr_debug("diag: In %s, SMD channel is closed, peripheral: %d\n",
__func__, smd_info->peripheral);
return;
}
mutex_lock(&event_mask.lock);
header.cmd_type = DIAG_CTRL_MSG_EVENT_MASK;
header.stream_id = 1;
header.status = event_mask.status;
switch (event_mask.status) {
case DIAG_CTRL_MASK_ALL_DISABLED:
header.event_config = 0;
header.event_mask_size = 0;
break;
case DIAG_CTRL_MASK_ALL_ENABLED:
header.event_config = 1;
header.event_mask_size = 0;
break;
case DIAG_CTRL_MASK_VALID:
header.event_config = 1;
header.event_mask_size = num_bytes;
if (num_bytes + sizeof(header) > event_mask.update_buf_len) {
temp_len = num_bytes + sizeof(header);
temp = krealloc(buf, temp_len, GFP_KERNEL);
if (!temp) {
pr_err("diag: Unable to realloc event mask update buffer\n");
goto err;
} else {
event_mask.update_buf = temp;
event_mask.update_buf_len = temp_len;
}
}
memcpy(buf + sizeof(header), event_mask.ptr, num_bytes);
write_len += num_bytes;
break;
default:
pr_debug("diag: In %s, invalid status %d\n", __func__,
event_mask.status);
goto err;
}
header.data_len = EVENT_MASK_CTRL_HEADER_LEN + header.event_mask_size;
memcpy(buf, &header, sizeof(header));
write_len += sizeof(header);
err = diag_smd_write(smd_info, buf, write_len);
if (err) {
pr_err("diag: Unable to send event masks to peripheral %d\n",
smd_info->peripheral);
}
err:
mutex_unlock(&event_mask.lock);
}
