evie/android_kernel_oneplus_msm8998
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Merge "diag: Split data stream for User PDs on LPASS"

Browse source
This commit is contained in:
Linux Build Service Account 2017-04-21 00:37:34 -07:00 • committed by Gerrit - the friendly Code Review server
commit c2b5adaf8a
8 changed files with 514 additions and 124 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
drivers/char/diag/diag_masks.c
View file

@ -457,7 +457,9 @@ static void diag_send_feature_mask_update(uint8_t peripheral)
if (driver->supports_apps_hdlc_encoding)
DIAG_SET_FEATURE_MASK(F_DIAG_APPS_HDLC_ENCODE);
if (driver->supports_apps_header_untagging) {
if (peripheral == PERIPHERAL_MODEM) {
if (peripheral == PERIPHERAL_MODEM ||
peripheral == PERIPHERAL_LPASS ||
peripheral == PERIPHERAL_CDSP) {
DIAG_SET_FEATURE_MASK(F_DIAG_PKT_HEADER_UNTAG);
driver->peripheral_untag[peripheral] =
ENABLE_PKT_HEADER_UNTAGGING;

94
drivers/char/diag/diag_memorydevice.c
View file

@ -129,6 +129,37 @@ void diag_md_close_all()
diag_ws_reset(DIAG_WS_MUX);
}
static int diag_md_get_peripheral(int ctxt)
{
int peripheral;
if (driver->num_pd_session) {
peripheral = GET_PD_CTXT(ctxt);
switch (peripheral) {
case UPD_WLAN:
case UPD_AUDIO:
case UPD_SENSORS:
break;
case DIAG_ID_MPSS:
case DIAG_ID_LPASS:
case DIAG_ID_CDSP:
default:
peripheral =
GET_BUF_PERIPHERAL(ctxt);
if (peripheral > NUM_PERIPHERALS)
peripheral = -EINVAL;
break;
}
} else {
/* Account for Apps data as well */
peripheral = GET_BUF_PERIPHERAL(ctxt);
if (peripheral > NUM_PERIPHERALS)
peripheral = -EINVAL;
}
return peripheral;
}
int diag_md_write(int id, unsigned char *buf, int len, int ctx)
{
int i;
@ -144,26 +175,13 @@ int diag_md_write(int id, unsigned char *buf, int len, int ctx)
if (!buf || len < 0)
return -EINVAL;
if (driver->pd_logging_mode) {
peripheral = GET_PD_CTXT(ctx);
switch (peripheral) {
case UPD_WLAN:
break;
case DIAG_ID_MPSS:
default:
peripheral = GET_BUF_PERIPHERAL(ctx);
if (peripheral > NUM_PERIPHERALS)
return -EINVAL;
break;
}
} else {
/* Account for Apps data as well */
peripheral = GET_BUF_PERIPHERAL(ctx);
if (peripheral > NUM_PERIPHERALS)
return -EINVAL;
}
peripheral =
diag_md_get_peripheral(ctx);
if (peripheral < 0)
return -EINVAL;
session_info = diag_md_session_get_peripheral(peripheral);
session_info =
diag_md_session_get_peripheral(peripheral);
if (!session_info)
return -EIO;
@ -243,31 +261,15 @@ int diag_md_copy_to_user(char __user *buf, int *pret, size_t buf_size,
entry = &ch->tbl[j];
if (entry->len <= 0)
continue;
if (driver->pd_logging_mode) {
peripheral = GET_PD_CTXT(entry->ctx);
switch (peripheral) {
case UPD_WLAN:
break;
case DIAG_ID_MPSS:
default:
peripheral =
GET_BUF_PERIPHERAL(entry->ctx);
if (peripheral > NUM_PERIPHERALS)
goto drop_data;
break;
}
} else {
/* Account for Apps data as well */
peripheral = GET_BUF_PERIPHERAL(entry->ctx);
if (peripheral > NUM_PERIPHERALS)
goto drop_data;
}
peripheral = diag_md_get_peripheral(entry->ctx);
if (peripheral < 0)
goto drop_data;
session_info =
diag_md_session_get_peripheral(peripheral);
if (!session_info) {
mutex_unlock(&driver->diagfwd_untag_mutex);
return -EIO;
goto drop_data;
}
if (session_info && info &&
@ -363,9 +365,15 @@ int diag_md_close_peripheral(int id, uint8_t peripheral)
spin_lock_irqsave(&ch->lock, flags);
for (i = 0; i < ch->num_tbl_entries && !found; i++) {
entry = &ch->tbl[i];
if ((GET_BUF_PERIPHERAL(entry->ctx) != peripheral) ||
(GET_PD_CTXT(entry->ctx) != peripheral))
continue;
if (peripheral > NUM_PERIPHERALS) {
if (GET_PD_CTXT(entry->ctx) != peripheral)
continue;
} else {
if (GET_BUF_PERIPHERAL(entry->ctx) !=
peripheral)
continue;
}
found = 1;
if (ch->ops && ch->ops->write_done) {
ch->ops->write_done(entry->buf, entry->len,

10
drivers/char/diag/diag_mux.c
View file

@ -27,7 +27,7 @@
#include "diag_mux.h"
#include "diag_usb.h"
#include "diag_memorydevice.h"
#include "diag_ipc_logging.h"
struct diag_mux_state_t *diag_mux;
static struct diag_logger_t usb_logger;
@ -146,7 +146,15 @@ int diag_mux_write(int proc, unsigned char *buf, int len, int ctx)
case DIAG_ID_MPSS:
upd = PERIPHERAL_MODEM;
break;
case DIAG_ID_LPASS:
upd = PERIPHERAL_LPASS;
break;
case DIAG_ID_CDSP:
upd = PERIPHERAL_CDSP;
break;
case UPD_WLAN:
case UPD_AUDIO:
case UPD_SENSORS:
break;
default:
pr_err("diag: invalid pd ctxt= %d\n", upd);

19
drivers/char/diag/diagchar.h
View file

@ -76,7 +76,9 @@
| DIAG_CON_LPASS | DIAG_CON_WCNSS \
| DIAG_CON_SENSORS | DIAG_CON_WDSP \
| DIAG_CON_CDSP)
#define DIAG_CON_UPD_ALL (DIAG_CON_UPD_WLAN)
#define DIAG_CON_UPD_ALL (DIAG_CON_UPD_WLAN \
| DIAG_CON_UPD_AUDIO \
| DIAG_CON_UPD_SENSORS)
#define DIAG_STM_MODEM 0x01
#define DIAG_STM_LPASS 0x02
@ -222,6 +224,10 @@
#define DIAG_ID_APPS 1
#define DIAG_ID_MPSS 2
#define DIAG_ID_WLAN 3
#define DIAG_ID_LPASS 4
#define DIAG_ID_CDSP 5
#define DIAG_ID_AUDIO 6
#define DIAG_ID_SENSORS 7
/* Number of sessions possible in Memory Device Mode. +1 for Apps data */
#define NUM_MD_SESSIONS (NUM_PERIPHERALS \
@ -598,10 +604,15 @@ struct diagchar_dev {
int in_busy_dcipktdata;
int logging_mode;
int logging_mask;
int pd_logging_mode;
int pd_logging_mode[NUM_UPD];
int pd_session_clear[NUM_UPD];
int num_pd_session;
int cpd_len_1;
int cpd_len_2;
int cpd_len_1[NUM_PERIPHERALS];
int cpd_len_2[NUM_PERIPHERALS];
int upd_len_1_a[NUM_PERIPHERALS];
int upd_len_1_b[NUM_PERIPHERALS];
int upd_len_2_a;
int upd_len_2_b;
int mask_check;
uint32_t md_session_mask;
uint8_t md_session_mode;

134
drivers/char/diag/diagchar_core.c
View file

@ -397,6 +397,10 @@ static uint32_t diag_translate_kernel_to_user_mask(uint32_t peripheral_mask)
ret |= DIAG_CON_CDSP;
if (peripheral_mask & MD_PERIPHERAL_MASK(UPD_WLAN))
ret |= DIAG_CON_UPD_WLAN;
if (peripheral_mask & MD_PERIPHERAL_MASK(UPD_AUDIO))
ret |= DIAG_CON_UPD_AUDIO;
if (peripheral_mask & MD_PERIPHERAL_MASK(UPD_SENSORS))
ret |= DIAG_CON_UPD_SENSORS;
return ret;
}
int diag_mask_param(void)
@ -426,8 +430,8 @@ void diag_clear_masks(struct diag_md_session_t *info)
static void diag_close_logging_process(const int pid)
{
int i;
int session_peripheral_mask;
int i, j;
int session_mask;
struct diag_md_session_t *session_info = NULL;
struct diag_logging_mode_param_t params;
@ -443,27 +447,34 @@ static void diag_close_logging_process(const int pid)
mutex_unlock(&driver->diag_maskclear_mutex);
mutex_lock(&driver->diagchar_mutex);
session_peripheral_mask = session_info->peripheral_mask;
session_mask = session_info->peripheral_mask;
diag_md_session_close(session_info);
mutex_unlock(&driver->diagchar_mutex);
for (i = 0; i < NUM_MD_SESSIONS; i++)
if (MD_PERIPHERAL_MASK(i) & session_peripheral_mask)
if (MD_PERIPHERAL_MASK(i) & session_mask)
diag_mux_close_peripheral(DIAG_LOCAL_PROC, i);
params.req_mode = USB_MODE;
params.mode_param = 0;
params.peripheral_mask =
diag_translate_kernel_to_user_mask(session_peripheral_mask);
if (driver->pd_logging_mode)
params.pd_mask =
diag_translate_kernel_to_user_mask(session_peripheral_mask);
diag_translate_kernel_to_user_mask(session_mask);
if (session_peripheral_mask & MD_PERIPHERAL_MASK(UPD_WLAN)) {
driver->pd_logging_mode--;
driver->num_pd_session--;
for (i = UPD_WLAN; i < NUM_MD_SESSIONS; i++) {
if (session_mask &
MD_PERIPHERAL_MASK(i)) {
j = i - UPD_WLAN;
driver->pd_session_clear[j] = 1;
driver->pd_logging_mode[j] = 0;
driver->num_pd_session -= 1;
params.pd_mask =
diag_translate_kernel_to_user_mask(session_mask);
} else
params.pd_mask = 0;
}
mutex_lock(&driver->diagchar_mutex);
diag_switch_logging(&params);
mutex_unlock(&driver->diagchar_mutex);
}
@ -1562,17 +1573,22 @@ static uint32_t diag_translate_mask(uint32_t peripheral_mask)
ret |= (1 << PERIPHERAL_CDSP);
if (peripheral_mask & DIAG_CON_UPD_WLAN)
ret |= (1 << UPD_WLAN);
if (peripheral_mask & DIAG_CON_UPD_AUDIO)
ret |= (1 << UPD_AUDIO);
if (peripheral_mask & DIAG_CON_UPD_SENSORS)
ret |= (1 << UPD_SENSORS);
return ret;
}
static int diag_switch_logging(struct diag_logging_mode_param_t *param)
{
int new_mode;
int new_mode, i;
int curr_mode;
int err = 0;
uint8_t do_switch = 1;
uint32_t peripheral_mask = 0;
uint8_t peripheral, upd;
if (!param)
return -EINVAL;
@ -1583,10 +1599,28 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
return -EINVAL;
}
switch (param->pd_mask) {
case DIAG_CON_UPD_WLAN:
if (driver->md_session_map[PERIPHERAL_MODEM] &&
(MD_PERIPHERAL_MASK(PERIPHERAL_MODEM) &
if (param->pd_mask) {
switch (param->pd_mask) {
case DIAG_CON_UPD_WLAN:
peripheral = PERIPHERAL_MODEM;
upd = UPD_WLAN;
break;
case DIAG_CON_UPD_AUDIO:
peripheral = PERIPHERAL_LPASS;
upd = UPD_AUDIO;
break;
case DIAG_CON_UPD_SENSORS:
peripheral = PERIPHERAL_LPASS;
upd = UPD_SENSORS;
break;
default:
DIAG_LOG(DIAG_DEBUG_USERSPACE,
"asking for mode switch with no pd mask set\n");
return -EINVAL;
}
if (driver->md_session_map[peripheral] &&
(MD_PERIPHERAL_MASK(peripheral) &
diag_mux->mux_mask)) {
DIAG_LOG(DIAG_DEBUG_USERSPACE,
"diag_fr: User PD is already logging onto active peripheral logging\n");
@ -1595,15 +1629,16 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
peripheral_mask =
diag_translate_mask(param->pd_mask);
param->peripheral_mask = peripheral_mask;
driver->pd_logging_mode++;
driver->num_pd_session++;
break;
default:
i = upd - UPD_WLAN;
if (!driver->pd_session_clear[i]) {
driver->pd_logging_mode[i] = 1;
driver->num_pd_session += 1;
driver->pd_session_clear[i] = 0;
}
} else {
peripheral_mask =
diag_translate_mask(param->peripheral_mask);
param->peripheral_mask = peripheral_mask;
break;
}
switch (param->req_mode) {
@ -1945,9 +1980,36 @@ static int diag_ioctl_hdlc_toggle(unsigned long ioarg)
return 0;
}
static int diag_ioctl_query_pd_logging(unsigned long ioarg)
static int diag_ioctl_query_pd_logging(struct diag_logging_mode_param_t *param)
{
int ret = -EINVAL;
int peripheral;
char *p_str = NULL;
if (!param)
return -EINVAL;
if (!param->pd_mask) {
DIAG_LOG(DIAG_DEBUG_USERSPACE,
"query with no pd mask set, returning error\n");
return -EINVAL;
}
switch (param->pd_mask) {
case DIAG_CON_UPD_WLAN:
peripheral = PERIPHERAL_MODEM;
p_str = "MODEM";
break;
case DIAG_CON_UPD_AUDIO:
case DIAG_CON_UPD_SENSORS:
peripheral = PERIPHERAL_LPASS;
p_str = "LPASS";
break;
default:
DIAG_LOG(DIAG_DEBUG_USERSPACE,
"Invalid pd mask, returning EINVAL\n");
return -EINVAL;
}
DIAG_LOG(DIAG_DEBUG_USERSPACE,
"diag: %s: Untagging support on APPS is %s\n", __func__,
@ -1955,12 +2017,13 @@ static int diag_ioctl_query_pd_logging(unsigned long ioarg)
"present" : "absent"));
DIAG_LOG(DIAG_DEBUG_USERSPACE,
"diag: %s: Tagging support on MODEM is %s\n", __func__,
(driver->feature[PERIPHERAL_MODEM].untag_header ?
"diag: %s: Tagging support on %s is %s\n",
__func__, p_str,
(driver->feature[peripheral].untag_header ?
"present" : "absent"));
if (driver->supports_apps_header_untagging &&
driver->feature[PERIPHERAL_MODEM].untag_header)
driver->feature[peripheral].untag_header)
ret = 0;
return ret;
@ -2206,7 +2269,10 @@ long diagchar_compat_ioctl(struct file *filp,
result = diag_ioctl_hdlc_toggle(ioarg);
break;
case DIAG_IOCTL_QUERY_PD_LOGGING:
result = diag_ioctl_query_pd_logging(ioarg);
if (copy_from_user((void *)&mode_param, (void __user *)ioarg,
sizeof(mode_param)))
return -EFAULT;
result = diag_ioctl_query_pd_logging(&mode_param);
break;
}
return result;
@ -2332,7 +2398,10 @@ long diagchar_ioctl(struct file *filp,
result = diag_ioctl_hdlc_toggle(ioarg);
break;
case DIAG_IOCTL_QUERY_PD_LOGGING:
result = diag_ioctl_query_pd_logging(ioarg);
if (copy_from_user((void *)&mode_param, (void __user *)ioarg,
sizeof(mode_param)))
return -EFAULT;
result = diag_ioctl_query_pd_logging(&mode_param);
break;
}
return result;
@ -3474,7 +3543,10 @@ static int __init diagchar_init(void)
poolsize_usb_apps + 1 + (NUM_PERIPHERALS * 6));
driver->num_clients = max_clients;
driver->logging_mode = DIAG_USB_MODE;
driver->pd_logging_mode = 0;
for (i = 0; i < NUM_UPD; i++) {
driver->pd_logging_mode[i] = 0;
driver->pd_session_clear[i] = 0;
}
driver->num_pd_session = 0;
driver->mask_check = 0;
driver->in_busy_pktdata = 0;

1
drivers/char/diag/diagfwd.c
View file

@ -38,6 +38,7 @@
#include "diag_masks.h"
#include "diag_usb.h"
#include "diag_mux.h"
#include "diag_ipc_logging.h"
#define STM_CMD_VERSION_OFFSET 4
#define STM_CMD_MASK_OFFSET 5

374
drivers/char/diag/diagfwd_peripheral.c
View file

@ -244,9 +244,14 @@ static void diagfwd_data_process_done(struct diagfwd_info *fwd_info,
mutex_lock(&driver->hdlc_disable_mutex);
mutex_lock(&fwd_info->data_mutex);
peripheral = GET_PD_CTXT(buf->ctxt);
if (peripheral == DIAG_ID_MPSS)
peripheral = PERIPHERAL_MODEM;
if (peripheral == DIAG_ID_LPASS)
peripheral = PERIPHERAL_LPASS;
if (peripheral == DIAG_ID_CDSP)
peripheral = PERIPHERAL_CDSP;
session_info =
diag_md_session_get_peripheral(peripheral);
@ -323,15 +328,19 @@ end:
static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
unsigned char *buf, int len)
{
int len_cpd = 0, len_upd_1 = 0;
int ctxt_cpd = 0, ctxt_upd_1 = 0;
int len_cpd = 0;
int len_upd_1 = 0, len_upd_2 = 0;
int ctxt_cpd = 0;
int ctxt_upd_1 = 0, ctxt_upd_2 = 0;
int buf_len = 0, processed = 0;
unsigned char *temp_buf_main = NULL;
unsigned char *temp_buf_cpd = NULL;
unsigned char *temp_buf_upd_1 = NULL;
unsigned char *temp_buf_upd_2 = NULL;
struct diagfwd_buf_t *temp_ptr_upd = NULL;
struct diagfwd_buf_t *temp_ptr_cpd = NULL;
int flag_buf_1 = 0, flag_buf_2 = 0;
uint8_t peripheral;
if (!fwd_info || !buf || len <= 0) {
diag_ws_release();
@ -349,24 +358,42 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
diag_ws_release();
return;
}
peripheral = fwd_info->peripheral;
if (driver->feature[fwd_info->peripheral].encode_hdlc &&
driver->feature[fwd_info->peripheral].untag_header &&
driver->peripheral_untag[fwd_info->peripheral]) {
if (driver->feature[peripheral].encode_hdlc &&
driver->feature[peripheral].untag_header &&
driver->peripheral_untag[peripheral]) {
mutex_lock(&driver->diagfwd_untag_mutex);
temp_buf_cpd = buf;
temp_buf_main = buf;
if (fwd_info->buf_1 &&
fwd_info->buf_1->data_raw == buf) {
flag_buf_1 = 1;
if (fwd_info->type == TYPE_DATA)
temp_ptr_cpd = fwd_info->buf_1;
if (fwd_info->type == TYPE_DATA) {
temp_buf_upd_1 =
fwd_info->buf_upd_1_a->data_raw;
} else {
if (peripheral ==
PERIPHERAL_LPASS)
temp_buf_upd_2 =
fwd_info->buf_upd_2_a->data_raw;
}
} else if (fwd_info->buf_2 &&
fwd_info->buf_2->data_raw == buf) {
flag_buf_2 = 1;
temp_ptr_cpd = fwd_info->buf_2;
if (fwd_info->type == TYPE_DATA)
temp_buf_upd_1 =
fwd_info->buf_upd_1_b->data_raw;
if (peripheral ==
PERIPHERAL_LPASS)
temp_buf_upd_2 =
fwd_info->buf_upd_2_b->data_raw;
} else {
pr_err("diag: In %s, no match for buffer %pK, peripheral %d, type: %d\n",
__func__, buf, peripheral,
fwd_info->type);
goto end;
}
while (processed < len) {
buf_len =
@ -390,31 +417,97 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
temp_buf_upd_1 += buf_len;
}
break;
case DIAG_ID_LPASS:
ctxt_cpd = DIAG_ID_LPASS;
len_cpd += buf_len;
if (temp_buf_cpd) {
memcpy(temp_buf_cpd,
(temp_buf_main + 4), buf_len);
temp_buf_cpd += buf_len;
}
break;
case DIAG_ID_AUDIO:
ctxt_upd_1 = UPD_AUDIO;
len_upd_1 += buf_len;
if (temp_buf_upd_1) {
memcpy(temp_buf_upd_1,
(temp_buf_main + 4), buf_len);
temp_buf_upd_1 += buf_len;
}
break;
case DIAG_ID_SENSORS:
ctxt_upd_2 = UPD_SENSORS;
len_upd_2 += buf_len;
if (temp_buf_upd_2) {
memcpy(temp_buf_upd_2,
(temp_buf_main + 4), buf_len);
temp_buf_upd_2 += buf_len;
}
break;
case DIAG_ID_CDSP:
ctxt_cpd = DIAG_ID_CDSP;
len_cpd += buf_len;
if (temp_buf_cpd) {
memcpy(temp_buf_cpd,
(temp_buf_main + 4), buf_len);
temp_buf_cpd += buf_len;
}
break;
default:
goto end;
}
len = len - 4;
temp_buf_main += (buf_len + 4);
processed += buf_len;
}
if (fwd_info->type == TYPE_DATA && len_upd_1) {
if (peripheral == PERIPHERAL_LPASS &&
fwd_info->type == TYPE_DATA && len_upd_2) {
if (flag_buf_1) {
driver->upd_len_2_a = len_upd_2;
temp_ptr_upd = fwd_info->buf_upd_2_a;
} else {
driver->upd_len_2_b = len_upd_2;
temp_ptr_upd = fwd_info->buf_upd_2_b;
}
temp_ptr_upd->ctxt &= 0x00FFFFFF;
temp_ptr_upd->ctxt |=
(SET_PD_CTXT(ctxt_upd_2));
atomic_set(&temp_ptr_upd->in_busy, 1);
diagfwd_data_process_done(fwd_info,
temp_ptr_upd, len_upd_2);
} else {
if (flag_buf_1)
driver->upd_len_2_a = 0;
if (flag_buf_2)
driver->upd_len_2_b = 0;
}
if (fwd_info->type == TYPE_DATA && len_upd_1) {
if (flag_buf_1) {
driver->upd_len_1_a[peripheral] =
len_upd_1;
temp_ptr_upd = fwd_info->buf_upd_1_a;
else
} else {
driver->upd_len_1_b[peripheral] =
len_upd_1;
temp_ptr_upd = fwd_info->buf_upd_1_b;
}
temp_ptr_upd->ctxt &= 0x00FFFFFF;
temp_ptr_upd->ctxt |=
(SET_PD_CTXT(ctxt_upd_1));
atomic_set(&temp_ptr_upd->in_busy, 1);
diagfwd_data_process_done(fwd_info,
temp_ptr_upd, len_upd_1);
} else {
if (flag_buf_1)
driver->upd_len_1_a[peripheral] = 0;
if (flag_buf_2)
driver->upd_len_1_b[peripheral] = 0;
}
if (len_cpd) {
if (flag_buf_1) {
driver->cpd_len_1 = len_cpd;
temp_ptr_cpd = fwd_info->buf_1;
} else {
driver->cpd_len_2 = len_cpd;
temp_ptr_cpd = fwd_info->buf_2;
}
if (flag_buf_1)
driver->cpd_len_1[peripheral] = len_cpd;
else
driver->cpd_len_2[peripheral] = len_cpd;
temp_ptr_cpd->ctxt &= 0x00FFFFFF;
temp_ptr_cpd->ctxt |=
(SET_PD_CTXT(ctxt_cpd));
@ -422,14 +515,24 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
temp_ptr_cpd, len_cpd);
} else {
if (flag_buf_1)
driver->cpd_len_1 = 0;
driver->cpd_len_1[peripheral] = 0;
if (flag_buf_2)
driver->cpd_len_2 = 0;
driver->cpd_len_2[peripheral] = 0;
}
mutex_unlock(&driver->diagfwd_untag_mutex);
return;
} else {
diagfwd_data_read_done(fwd_info, buf, len);
return;
}
end:
diag_ws_release();
mutex_unlock(&driver->diagfwd_untag_mutex);
if (temp_ptr_cpd) {
diagfwd_write_done(fwd_info->peripheral, fwd_info->type,
GET_BUF_NUM(temp_ptr_cpd->ctxt));
}
diagfwd_queue_read(fwd_info);
}
static void diagfwd_data_read_done(struct diagfwd_info *fwd_info,
@ -1166,20 +1269,78 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
return;
fwd_info = &peripheral_info[type][peripheral];
if (ctxt == 1 && fwd_info->buf_1) {
/* Buffer 1 for core PD is freed */
atomic_set(&fwd_info->buf_1->in_busy, 0);
driver->cpd_len_1 = 0;
driver->cpd_len_1[peripheral] = 0;
} else if (ctxt == 2 && fwd_info->buf_2) {
/* Buffer 2 for core PD is freed */
atomic_set(&fwd_info->buf_2->in_busy, 0);
driver->cpd_len_2 = 0;
driver->cpd_len_2[peripheral] = 0;
} else if (ctxt == 3 && fwd_info->buf_upd_1_a) {
/* Buffer 1 for user pd 1 is freed */
atomic_set(&fwd_info->buf_upd_1_a->in_busy, 0);
if (driver->cpd_len_1 == 0)
atomic_set(&fwd_info->buf_1->in_busy, 0);
if (peripheral == PERIPHERAL_LPASS) {
/* if not data in cpd and other user pd
* free the core pd buffer for LPASS
*/
if (!driver->cpd_len_1[PERIPHERAL_LPASS] &&
!driver->upd_len_2_a)
atomic_set(&fwd_info->buf_1->in_busy, 0);
} else {
/* if not data in cpd
* free the core pd buffer for MPSS
*/
if (!driver->cpd_len_1[PERIPHERAL_MODEM])
atomic_set(&fwd_info->buf_1->in_busy, 0);
}
driver->upd_len_1_a[peripheral] = 0;
} else if (ctxt == 4 && fwd_info->buf_upd_1_b) {
/* Buffer 2 for user pd 1 is freed */
atomic_set(&fwd_info->buf_upd_1_b->in_busy, 0);
if (driver->cpd_len_2 == 0)
if (peripheral == PERIPHERAL_LPASS) {
/* if not data in cpd and other user pd
* free the core pd buffer for LPASS
*/
if (!driver->cpd_len_2[peripheral] &&
!driver->upd_len_2_b)
atomic_set(&fwd_info->buf_2->in_busy, 0);
} else {
/* if not data in cpd
* free the core pd buffer for MPSS
*/
if (!driver->cpd_len_2[PERIPHERAL_MODEM])
atomic_set(&fwd_info->buf_2->in_busy, 0);
}
driver->upd_len_1_b[peripheral] = 0;
} else if (ctxt == 5 && fwd_info->buf_upd_2_a) {
/* Buffer 1 for user pd 2 is freed */
atomic_set(&fwd_info->buf_upd_2_a->in_busy, 0);
/* if not data in cpd and other user pd
* free the core pd buffer for LPASS
*/
if (!driver->cpd_len_1[PERIPHERAL_LPASS] &&
!driver->upd_len_1_a[PERIPHERAL_LPASS])
atomic_set(&fwd_info->buf_1->in_busy, 0);
driver->upd_len_2_a = 0;
} else if (ctxt == 6 && fwd_info->buf_upd_2_b) {
/* Buffer 2 for user pd 2 is freed */
atomic_set(&fwd_info->buf_upd_2_b->in_busy, 0);
/* if not data in cpd and other user pd
* free the core pd buffer for LPASS
*/
if (!driver->cpd_len_2[PERIPHERAL_LPASS] &&
!driver->upd_len_1_b[PERIPHERAL_LPASS])
atomic_set(&fwd_info->buf_2->in_busy, 0);
driver->upd_len_2_b = 0;
} else
pr_err("diag: In %s, invalid ctxt %d\n", __func__, ctxt);
@ -1312,7 +1473,8 @@ static void diagfwd_queue_read(struct diagfwd_info *fwd_info)
void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
{
unsigned char *temp_buf = NULL;
struct diagfwd_buf_t *temp_fwd_buf;
unsigned char *temp_char_buf;
if (!fwd_info)
return;
@ -1383,11 +1545,11 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_buf = fwd_info->buf_upd_1_a->data;
if (ZERO_OR_NULL_PTR(temp_buf))
temp_char_buf = fwd_info->buf_upd_1_a->data;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_1_a->len = PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_buf);
kmemleak_not_leak(temp_char_buf);
fwd_info->buf_upd_1_a->ctxt = SET_BUF_CTXT(
fwd_info->peripheral,
fwd_info->type, 3);
@ -1408,16 +1570,76 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_buf = fwd_info->buf_upd_1_b->data;
if (ZERO_OR_NULL_PTR(temp_buf))
temp_char_buf =
fwd_info->buf_upd_1_b->data;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_1_b->len =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_buf);
kmemleak_not_leak(temp_char_buf);
fwd_info->buf_upd_1_b->ctxt = SET_BUF_CTXT(
fwd_info->peripheral,
fwd_info->type, 4);
}
if (fwd_info->peripheral ==
PERIPHERAL_LPASS) {
if (!fwd_info->buf_upd_2_a) {
fwd_info->buf_upd_2_a =
kzalloc(sizeof(struct diagfwd_buf_t),
GFP_KERNEL);
temp_fwd_buf =
fwd_info->buf_upd_2_a;
if (ZERO_OR_NULL_PTR(temp_fwd_buf))
goto err;
kmemleak_not_leak(temp_fwd_buf);
}
if (!fwd_info->buf_upd_2_a->data) {
fwd_info->buf_upd_2_a->data =
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_char_buf =
fwd_info->buf_upd_2_a->data;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_2_a->len =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_char_buf);
fwd_info->buf_upd_2_a->ctxt =
SET_BUF_CTXT(
fwd_info->peripheral,
fwd_info->type, 5);
}
if (!fwd_info->buf_upd_2_b) {
fwd_info->buf_upd_2_b =
kzalloc(sizeof(struct diagfwd_buf_t),
GFP_KERNEL);
temp_fwd_buf =
fwd_info->buf_upd_2_b;
if (ZERO_OR_NULL_PTR(temp_fwd_buf))
goto err;
kmemleak_not_leak(temp_fwd_buf);
}
if (!fwd_info->buf_upd_2_b->data) {
fwd_info->buf_upd_2_b->data =
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_char_buf =
fwd_info->buf_upd_2_b->data;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_2_b->len =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_char_buf);
fwd_info->buf_upd_2_b->ctxt =
SET_BUF_CTXT(
fwd_info->peripheral,
fwd_info->type, 6);
}
}
}
if (driver->supports_apps_hdlc_encoding) {
@ -1427,12 +1649,13 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_buf = fwd_info->buf_1->data_raw;
if (ZERO_OR_NULL_PTR(temp_buf))
temp_char_buf =
fwd_info->buf_1->data_raw;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_1->len_raw =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_buf);
kmemleak_not_leak(temp_char_buf);
}
if (!fwd_info->buf_2->data_raw) {
@ -1440,12 +1663,13 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_buf = fwd_info->buf_2->data_raw;
if (ZERO_OR_NULL_PTR(temp_buf))
temp_char_buf =
fwd_info->buf_2->data_raw;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_2->len_raw =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_buf);
kmemleak_not_leak(temp_char_buf);
}
if (driver->feature[fwd_info->peripheral].
@ -1456,13 +1680,13 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_buf =
temp_char_buf =
fwd_info->buf_upd_1_a->data_raw;
if (ZERO_OR_NULL_PTR(temp_buf))
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_1_a->len_raw =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_buf);
kmemleak_not_leak(temp_char_buf);
}
if (fwd_info->buf_upd_1_b &&
@ -1471,13 +1695,41 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_buf =
temp_char_buf =
fwd_info->buf_upd_1_b->data_raw;
if (ZERO_OR_NULL_PTR(temp_buf))
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_1_b->len_raw =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_buf);
kmemleak_not_leak(temp_char_buf);
}
if (fwd_info->peripheral == PERIPHERAL_LPASS
&& !fwd_info->buf_upd_2_a->data_raw) {
fwd_info->buf_upd_2_a->data_raw =
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_char_buf =
fwd_info->buf_upd_2_a->data_raw;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_2_a->len_raw =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_char_buf);
}
if (fwd_info->peripheral == PERIPHERAL_LPASS
&& !fwd_info->buf_upd_2_b->data_raw) {
fwd_info->buf_upd_2_b->data_raw =
kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
temp_char_buf =
fwd_info->buf_upd_2_b->data_raw;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_upd_2_b->len_raw =
PERIPHERAL_BUF_SZ;
kmemleak_not_leak(temp_char_buf);
}
}
}
@ -1490,10 +1742,12 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
fwd_info->buf_1->data_raw = kzalloc(PERIPHERAL_BUF_SZ +
APF_DIAG_PADDING,
GFP_KERNEL);
if (!fwd_info->buf_1->data_raw)
temp_char_buf =
fwd_info->buf_1->data_raw;
if (ZERO_OR_NULL_PTR(temp_char_buf))
goto err;
fwd_info->buf_1->len_raw = PERIPHERAL_BUF_SZ;
kmemleak_not_leak(fwd_info->buf_1->data_raw);
kmemleak_not_leak(temp_char_buf);
}
}
@ -1530,6 +1784,38 @@ static void diagfwd_buffers_exit(struct diagfwd_info *fwd_info)
kfree(fwd_info->buf_2);
fwd_info->buf_2 = NULL;
}
if (fwd_info->buf_upd_1_a) {
kfree(fwd_info->buf_upd_1_a->data);
fwd_info->buf_upd_1_a->data = NULL;
kfree(fwd_info->buf_upd_1_a->data_raw);
fwd_info->buf_upd_1_a->data_raw = NULL;
kfree(fwd_info->buf_upd_1_a);
fwd_info->buf_upd_1_a = NULL;
}
if (fwd_info->buf_upd_1_b) {
kfree(fwd_info->buf_upd_1_b->data);
fwd_info->buf_upd_1_b->data = NULL;
kfree(fwd_info->buf_upd_1_b->data_raw);
fwd_info->buf_upd_1_b->data_raw = NULL;
kfree(fwd_info->buf_upd_1_b);
fwd_info->buf_upd_1_b = NULL;
}
if (fwd_info->buf_upd_2_a) {
kfree(fwd_info->buf_upd_2_a->data);
fwd_info->buf_upd_2_a->data = NULL;
kfree(fwd_info->buf_upd_2_a->data_raw);
fwd_info->buf_upd_2_a->data_raw = NULL;
kfree(fwd_info->buf_upd_2_a);
fwd_info->buf_upd_2_a = NULL;
}
if (fwd_info->buf_upd_2_b) {
kfree(fwd_info->buf_upd_2_b->data);
fwd_info->buf_upd_2_b->data = NULL;
kfree(fwd_info->buf_upd_2_b->data_raw);
fwd_info->buf_upd_2_b->data_raw = NULL;
kfree(fwd_info->buf_upd_2_b);
fwd_info->buf_upd_2_b = NULL;
}
mutex_unlock(&fwd_info->buf_mutex);
}

2
drivers/char/diag/diagfwd_peripheral.h
View file

@ -80,6 +80,8 @@ struct diagfwd_info {
struct diagfwd_buf_t *buf_2;
struct diagfwd_buf_t *buf_upd_1_a;
struct diagfwd_buf_t *buf_upd_1_b;
struct diagfwd_buf_t *buf_upd_2_a;
struct diagfwd_buf_t *buf_upd_2_b;
struct diagfwd_buf_t *buf_ptr[NUM_WRITE_BUFFERS];
struct diag_peripheral_ops *p_ops;
struct diag_channel_ops *c_ops;