static int diag_create_msg_mask_table(void)
{
int i;
int err = 0;
struct diag_msg_mask_t *mask = (struct diag_msg_mask_t *)msg_mask.ptr;
struct diag_ssid_range_t range;
mutex_lock(&msg_mask.lock);
driver->msg_mask_tbl_count = MSG_MASK_TBL_CNT;
for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
range.ssid_first = msg_mask_tbl[i].ssid_first;
range.ssid_last = msg_mask_tbl[i].ssid_last;
err = diag_create_msg_mask_table_entry(mask, &range);
if (err)
break;
}
mutex_unlock(&msg_mask.lock);
return err;
}
static void diag_build_time_mask_update(uint8_t *buf,
struct diag_ssid_range_t *range)
{
int i;
int j;
int num_items = 0;
int err = 0;
int found = 0;
int new_size = 0;
uint8_t *temp = NULL;
uint32_t *mask_ptr = (uint32_t *)buf;
uint32_t *dest_ptr = NULL;
struct diag_msg_mask_t *build_mask = NULL;
if (!range || !buf)
return;
if (range->ssid_last < range->ssid_first) {
pr_err("diag: In %s, invalid ssid range, first: %d, last: %d\n",
__func__, range->ssid_first, range->ssid_last);
return;
}
build_mask = (struct diag_msg_mask_t *)(driver->build_time_mask->ptr);
num_items = range->ssid_last - range->ssid_first + 1;
mutex_lock(&driver->build_time_mask->lock);
for (i = 0; i < driver->msg_mask_tbl_count; i++, build_mask++) {
if (build_mask->ssid_first != range->ssid_first)
continue;
found = 1;
err = update_msg_mask_tbl_entry(build_mask, range);
if (err == -ENOMEM) {
pr_err("diag: In %s, unable to increase the msg build mask table range\n",
__func__);
}
dest_ptr = build_mask->ptr;
for (j = 0; j < build_mask->range; j++, mask_ptr++, dest_ptr++)
*(uint32_t *)dest_ptr |= *mask_ptr;
break;
}
if (found)
goto end;
new_size = (driver->msg_mask_tbl_count + 1) *
sizeof(struct diag_msg_mask_t);
temp = krealloc(driver->build_time_mask->ptr, new_size, GFP_KERNEL);
if (!temp) {
pr_err("diag: In %s, unable to create a new entry for build time mask\n",
__func__);
goto end;
}
driver->build_time_mask->ptr = temp;
err = diag_create_msg_mask_table_entry(build_mask, range);
if (err) {
pr_err("diag: In %s, Unable to create a new msg mask table entry, err: %d\n",
__func__, err);
goto end;
}
driver->msg_mask_tbl_count += 1;
end:
mutex_unlock(&driver->build_time_mask->lock);
}
static void process_ssid_range_report(uint8_t *buf, uint32_t len,
struct diag_smd_info *smd_info)
{
int i;
int j;
int read_len = 0;
int found = 0;
int new_size = 0;
int err = 0;
struct diag_ctrl_ssid_range_report *header = NULL;
struct diag_ssid_range_t *ssid_range = NULL;
int header_len = sizeof(struct diag_ctrl_ssid_range_report);
struct diag_msg_mask_t *mask_ptr = NULL;
uint8_t *ptr = buf;
uint8_t *temp = NULL;
uint32_t min_len = header_len - sizeof(struct diag_ctrl_pkt_header_t);
if (!buf || !smd_info || len < min_len)
return;
header = (struct diag_ctrl_ssid_range_report *)ptr;
ptr += header_len;
/* Don't account for pkt_id and length */
read_len += header_len - (2 * sizeof(uint32_t));
mutex_lock(&msg_mask.lock);
driver->max_ssid_count[smd_info->peripheral] = header->count;
for (i = 0; i < header->count && read_len < len; i++) {
ssid_range = (struct diag_ssid_range_t *)ptr;
ptr += sizeof(struct diag_ssid_range_t);
read_len += sizeof(struct diag_ssid_range_t);
mask_ptr = (struct diag_msg_mask_t *)msg_mask.ptr;
found = 0;
for (j = 0; j < driver->msg_mask_tbl_count; j++, mask_ptr++) {
if (mask_ptr->ssid_first != ssid_range->ssid_first)
continue;
err = update_msg_mask_tbl_entry(mask_ptr, ssid_range);
if (err == -ENOMEM) {
pr_err("diag: In %s, unable to increase the msg mask table range\n",
__func__);
}
found = 1;
break;
}
if (found)
continue;
new_size = (driver->msg_mask_tbl_count + 1) *
sizeof(struct diag_msg_mask_t);
temp = krealloc(msg_mask.ptr, new_size, GFP_KERNEL);
if (!temp) {
pr_err("diag: In %s, Unable to add new ssid table to msg mask, ssid first: %d, last: %d\n",
__func__, ssid_range->ssid_first,
ssid_range->ssid_last);
continue;
}
msg_mask.ptr = temp;
err = diag_create_msg_mask_table_entry(mask_ptr, ssid_range);
if (err) {
pr_err("diag: In %s, Unable to create a new msg mask table entry, first: %d last: %d err: %d\n",
__func__, ssid_range->ssid_first,
ssid_range->ssid_last, err);
continue;
}
driver->msg_mask_tbl_count += 1;
}
mutex_unlock(&msg_mask.lock);
}
static int diag_create_build_time_mask(void)
{
int i;
int err = 0;
const uint32_t *tbl = NULL;
uint32_t tbl_size = 0;
struct diag_msg_mask_t *build_mask = NULL;
struct diag_ssid_range_t range;
mutex_lock(&msg_bt_mask.lock);
build_mask = (struct diag_msg_mask_t *)msg_bt_mask.ptr;
for (i = 0; i < driver->msg_mask_tbl_count; i++, build_mask++) {
range.ssid_first = msg_mask_tbl[i].ssid_first;
range.ssid_last = msg_mask_tbl[i].ssid_last;
err = diag_create_msg_mask_table_entry(build_mask, &range);
if (err)
break;
switch (build_mask->ssid_first) {
case MSG_SSID_0:
tbl = msg_bld_masks_0;
tbl_size = sizeof(msg_bld_masks_0);
break;
case MSG_SSID_1:
tbl = msg_bld_masks_1;
tbl_size = sizeof(msg_bld_masks_1);
break;
case MSG_SSID_2:
tbl = msg_bld_masks_2;
tbl_size = sizeof(msg_bld_masks_2);
break;
case MSG_SSID_3:
tbl = msg_bld_masks_3;
tbl_size = sizeof(msg_bld_masks_3);
break;
case MSG_SSID_4:
tbl = msg_bld_masks_4;
tbl_size = sizeof(msg_bld_masks_4);
break;
case MSG_SSID_5:
tbl = msg_bld_masks_5;
tbl_size = sizeof(msg_bld_masks_5);
break;
case MSG_SSID_6:
tbl = msg_bld_masks_6;
tbl_size = sizeof(msg_bld_masks_6);
break;
case MSG_SSID_7:
tbl = msg_bld_masks_7;
tbl_size = sizeof(msg_bld_masks_7);
break;
case MSG_SSID_8:
tbl = msg_bld_masks_8;
tbl_size = sizeof(msg_bld_masks_8);
break;
case MSG_SSID_9:
tbl = msg_bld_masks_9;
tbl_size = sizeof(msg_bld_masks_9);
break;
case MSG_SSID_10:
tbl = msg_bld_masks_10;
tbl_size = sizeof(msg_bld_masks_10);
break;
case MSG_SSID_11:
tbl = msg_bld_masks_11;
tbl_size = sizeof(msg_bld_masks_11);
break;
case MSG_SSID_12:
tbl = msg_bld_masks_12;
tbl_size = sizeof(msg_bld_masks_12);
break;
case MSG_SSID_13:
tbl = msg_bld_masks_13;
tbl_size = sizeof(msg_bld_masks_13);
break;
case MSG_SSID_14:
tbl = msg_bld_masks_14;
tbl_size = sizeof(msg_bld_masks_14);
break;
case MSG_SSID_15:
tbl = msg_bld_masks_15;
tbl_size = sizeof(msg_bld_masks_15);
break;
case MSG_SSID_16:
tbl = msg_bld_masks_16;
tbl_size = sizeof(msg_bld_masks_16);
break;
case MSG_SSID_17:
tbl = msg_bld_masks_17;
tbl_size = sizeof(msg_bld_masks_17);
break;
case MSG_SSID_18:
tbl = msg_bld_masks_18;
tbl_size = sizeof(msg_bld_masks_18);
break;
case MSG_SSID_19:
tbl = msg_bld_masks_19;
tbl_size = sizeof(msg_bld_masks_19);
break;
case MSG_SSID_20:
tbl = msg_bld_masks_20;
tbl_size = sizeof(msg_bld_masks_20);
break;
case MSG_SSID_21:
tbl = msg_bld_masks_21;
tbl_size = sizeof(msg_bld_masks_21);
break;
case MSG_SSID_22:
tbl = msg_bld_masks_22;
tbl_size = sizeof(msg_bld_masks_22);
break;
}
if (!tbl)
continue;
if (tbl_size > build_mask->range * sizeof(uint32_t)) {
pr_warn("diag: In %s, table %d has more ssid than max, ssid_first: %d, ssid_last: %d\n",
__func__, i, build_mask->ssid_first,
build_mask->ssid_last);
tbl_size = build_mask->range * sizeof(uint32_t);
}
memcpy(build_mask->ptr, tbl, tbl_size);
}
mutex_unlock(&msg_bt_mask.lock);
return err;
}
