/**
* This function provides NAS-QoS priority_group CPS API read function
* @Param Standard CPS API params
* @Return Standard Error Code
*/
cps_api_return_code_t nas_qos_cps_api_priority_group_read (void * context,
cps_api_get_params_t * param,
size_t ix)
{
cps_api_object_t obj = cps_api_object_list_get(param->filters, ix);
cps_api_object_attr_t port_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_PORT_ID);
cps_api_object_attr_t local_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_LOCAL_ID);
uint_t switch_id = 0;
if (port_id_attr == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Port Id must be specified\n");
return NAS_QOS_E_MISSING_KEY;
}
uint_t port_id = cps_api_object_attr_data_u32(port_id_attr);
bool local_id_specified = false;
uint8_t local_id;
if (local_id_attr) {
local_id = *(uint8_t *)cps_api_object_attr_data_bin(local_id_attr);
local_id_specified = true;
}
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Read switch id %u, port_id id %u\n",
switch_id, port_id);
std_mutex_simple_lock_guard p_m(&priority_group_mutex);
// If the port priority group is not initialized yet, initialize it in NAS
nas_qos_port_priority_group_init(port_id);
return nas_qos_cps_get_priority_group_info(param, switch_id, port_id,
local_id_specified, local_id);
}
/**
* This function provides NAS-QoS priority_group CPS API rollback function
* @Param Standard CPS API params
* @Return Standard Error Code
*/
cps_api_return_code_t nas_qos_cps_api_priority_group_rollback(void * context,
cps_api_transaction_params_t * param,
size_t ix)
{
cps_api_object_t obj = cps_api_object_list_get(param->prev,ix);
cps_api_operation_types_t op = cps_api_object_type_operation(cps_api_object_key(obj));
std_mutex_simple_lock_guard p_m(&priority_group_mutex);
if (op == cps_api_oper_SET) {
nas_qos_cps_api_priority_group_set(obj, NULL);
}
return cps_api_ret_code_OK;
}
/**
* Initializes the sampel sets so that the first update works appropriately
*/
MCL::MCL()
{
// Initialize particles to be randomly spread about the field...
srand(time(NULL));
for (int m = 0; m < M; ++m) {
//Particle p_m;
// X bounded by width of the field
// Y bounded by height of the field
// H between +-pi
PoseEst x_m(float(rand() % int(FIELD_WIDTH)),
float(rand() % int(FIELD_HEIGHT)),
float(((rand() % FULL_CIRC) - HALF_CIRC))*DEG_TO_RAD);
Particle p_m(x_m, 1.0f);
// p_m.pose = x_m;
// p_m.weight = 1;
X_t.push_back(p_m);
}
updateEstimates();
}
/**
* This function provides NAS-QoS priority_group CPS API write function
* @Param Standard CPS API params
* @Return Standard Error Code
*/
cps_api_return_code_t nas_qos_cps_api_priority_group_write(void * context,
cps_api_transaction_params_t * param,
size_t ix)
{
cps_api_object_t obj = cps_api_object_list_get(param->change_list,ix);
cps_api_operation_types_t op = cps_api_object_type_operation(cps_api_object_key(obj));
std_mutex_simple_lock_guard p_m(&priority_group_mutex);
switch (op) {
case cps_api_oper_CREATE:
case cps_api_oper_DELETE:
return NAS_QOS_E_FAIL; //not supported
case cps_api_oper_SET:
return nas_qos_cps_api_priority_group_set(obj, param->prev);
default:
return NAS_QOS_E_UNSUPPORTED;
}
}
template<typename Scalar, int Dim, int Options> void transform_products()
{
typedef Matrix<Scalar,Dim+1,Dim+1> Mat;
typedef Transform<Scalar,Dim,Projective,Options> Proj;
typedef Transform<Scalar,Dim,Affine,Options> Aff;
typedef Transform<Scalar,Dim,AffineCompact,Options> AffC;
Proj p; p.matrix().setRandom();
Aff a; a.linear().setRandom(); a.translation().setRandom();
AffC ac = a;
Mat p_m(p.matrix()), a_m(a.matrix());
VERIFY_IS_APPROX((p*p).matrix(), p_m*p_m);
VERIFY_IS_APPROX((a*a).matrix(), a_m*a_m);
VERIFY_IS_APPROX((p*a).matrix(), p_m*a_m);
VERIFY_IS_APPROX((a*p).matrix(), a_m*p_m);
VERIFY_IS_APPROX((ac*a).matrix(), a_m*a_m);
VERIFY_IS_APPROX((a*ac).matrix(), a_m*a_m);
VERIFY_IS_APPROX((p*ac).matrix(), p_m*a_m);
VERIFY_IS_APPROX((ac*p).matrix(), a_m*p_m);
}
/**
* This function provides NAS-QoS priority_group stats CPS API clear function
* To clear the priority_group stats, set relevant counters to zero
* @Param Standard CPS API params
* @Return Standard Error Code
*/
cps_api_return_code_t nas_qos_cps_api_priority_group_stat_clear (void * context,
cps_api_transaction_params_t * param,
size_t ix)
{
cps_api_object_t obj = cps_api_object_list_get(param->change_list,ix);
cps_api_operation_types_t op = cps_api_object_type_operation(cps_api_object_key(obj));
if (op != cps_api_oper_SET)
return NAS_QOS_E_UNSUPPORTED;
cps_api_object_attr_t port_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID);
cps_api_object_attr_t local_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID);
if (port_id_attr == NULL ||
local_id_attr == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
"Incomplete key: port-id, priority_group local id must be specified\n");
return NAS_QOS_E_MISSING_KEY;
}
uint32_t switch_id = 0;
nas_qos_priority_group_key_t key;
key.port_id = cps_api_object_attr_data_u32(port_id_attr);
key.local_id = *(uint8_t *)cps_api_object_attr_data_bin(local_id_attr);
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
"Read switch id %u, port_id %u priority_group local_id %u stat\n",
switch_id, key.port_id, key.local_id);
std_mutex_simple_lock_guard p_m(&priority_group_mutex);
// If the port is not be initialized yet, initialize it in NAS
nas_qos_port_priority_group_init(key.port_id);
nas_qos_switch *switch_p = nas_qos_get_switch(switch_id);
if (switch_p == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "switch_id %u not found", switch_id);
return NAS_QOS_E_FAIL;
}
nas_qos_priority_group * priority_group_p = switch_p->get_priority_group(key);
if (priority_group_p == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group not found");
return NAS_QOS_E_FAIL;
}
std::vector<BASE_QOS_PRIORITY_GROUP_STAT_t> counter_ids;
cps_api_object_it_t it;
cps_api_object_it_begin(obj,&it);
for ( ; cps_api_object_it_valid(&it) ; cps_api_object_it_next(&it) ) {
cps_api_attr_id_t id = cps_api_object_attr_id(it.attr);
switch (id) {
case BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID:
case BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID:
break;
case BASE_QOS_PRIORITY_GROUP_STAT_PACKETS:
case BASE_QOS_PRIORITY_GROUP_STAT_BYTES:
case BASE_QOS_PRIORITY_GROUP_STAT_WATERMARK_BYTES:
counter_ids.push_back((BASE_QOS_PRIORITY_GROUP_STAT_t)id);
break;
case BASE_QOS_PRIORITY_GROUP_STAT_CURRENT_OCCUPANCY_BYTES:
// READ-only
break;
default:
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
"Unknown priority_group STAT flag: %u, ignored", id);
break;
}
}
if (ndi_qos_clear_priority_group_stats(priority_group_p->get_ndi_port_id(),
priority_group_p->ndi_obj_id(),
&counter_ids[0],
counter_ids.size()) != STD_ERR_OK) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group stats clear failed");
return NAS_QOS_E_FAIL;
}
return cps_api_ret_code_OK;
}
/**
* This function provides NAS-QoS priority_group stats CPS API read function
* @Param Standard CPS API params
* @Return Standard Error Code
*/
cps_api_return_code_t nas_qos_cps_api_priority_group_stat_read (void * context,
cps_api_get_params_t * param,
size_t ix)
{
cps_api_object_t obj = cps_api_object_list_get(param->filters, ix);
cps_api_object_attr_t port_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID);
cps_api_object_attr_t local_id_attr = cps_api_get_key_data(obj, BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID);
if (port_id_attr == NULL ||
local_id_attr == NULL ) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
"Incomplete key: port-id, priority_group local id must be specified\n");
return NAS_QOS_E_MISSING_KEY;
}
uint32_t switch_id = 0;
nas_qos_priority_group_key_t key;
key.port_id = cps_api_object_attr_data_u32(port_id_attr);
key.local_id = *(uint8_t *)cps_api_object_attr_data_bin(local_id_attr);
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS",
"Read switch id %u, port_id %u priority_group local id %u stat\n",
switch_id, key.port_id, key.local_id);
std_mutex_simple_lock_guard p_m(&priority_group_mutex);
// If the port is not be initialized yet, initialize it in NAS
nas_qos_port_priority_group_init(key.port_id);
nas_qos_switch *switch_p = nas_qos_get_switch(switch_id);
if (switch_p == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "switch_id %u not found", switch_id);
return NAS_QOS_E_FAIL;
}
nas_qos_priority_group * priority_group_p = switch_p->get_priority_group(key);
if (priority_group_p == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group not found");
return NAS_QOS_E_FAIL;
}
nas_qos_priority_group_stat_counter_t stats = {0};
std::vector<BASE_QOS_PRIORITY_GROUP_STAT_t> counter_ids;
cps_api_object_it_t it;
cps_api_object_it_begin(obj,&it);
for ( ; cps_api_object_it_valid(&it) ; cps_api_object_it_next(&it) ) {
cps_api_attr_id_t id = cps_api_object_attr_id(it.attr);
switch (id) {
case BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID:
case BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID:
break;
case BASE_QOS_PRIORITY_GROUP_STAT_PACKETS:
case BASE_QOS_PRIORITY_GROUP_STAT_BYTES:
case BASE_QOS_PRIORITY_GROUP_STAT_CURRENT_OCCUPANCY_BYTES:
case BASE_QOS_PRIORITY_GROUP_STAT_WATERMARK_BYTES:
counter_ids.push_back((BASE_QOS_PRIORITY_GROUP_STAT_t)id);
break;
default:
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Unknown priority_group STAT flag: %u, ignored", id);
break;
}
}
if (ndi_qos_get_priority_group_stats(priority_group_p->get_ndi_port_id(),
priority_group_p->ndi_obj_id(),
&counter_ids[0],
counter_ids.size(),
&stats) != STD_ERR_OK) {
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Priority Group stats get failed");
return NAS_QOS_E_FAIL;
}
// return stats objects to cps-app
cps_api_object_t ret_obj = cps_api_object_list_create_obj_and_append(param->list);
if (ret_obj == NULL) {
return cps_api_ret_code_ERR;
}
cps_api_key_from_attr_with_qual(cps_api_object_key(ret_obj),
BASE_QOS_PRIORITY_GROUP_STAT_OBJ,
cps_api_qualifier_TARGET);
cps_api_set_key_data(ret_obj, BASE_QOS_PRIORITY_GROUP_STAT_PORT_ID,
cps_api_object_ATTR_T_U32,
&(key.port_id), sizeof(uint32_t));
cps_api_set_key_data(ret_obj, BASE_QOS_PRIORITY_GROUP_STAT_LOCAL_ID,
cps_api_object_ATTR_T_BIN,
&(key.local_id), sizeof(uint8_t));
uint64_t val64;
for (uint_t i=0; i< counter_ids.size(); i++) {
BASE_QOS_PRIORITY_GROUP_STAT_t id = counter_ids[i];
val64 = get_stats_by_type(&stats, id);
cps_api_object_attr_add_u64(ret_obj, id, val64);
}
return cps_api_ret_code_OK;
}
/**
* This function provides NAS-QoS SCHEDULER_GROUP CPS API read function
* @Param Standard CPS API params
* @Return Standard Error Code
*/
cps_api_return_code_t nas_qos_cps_api_scheduler_group_read (void * context,
cps_api_get_params_t * param,
size_t ix)
{
cps_api_object_t obj = cps_api_object_list_get(param->filters, ix);
cps_api_object_attr_t sg_attr = cps_api_get_key_data(obj, BASE_QOS_SCHEDULER_GROUP_ID);
uint32_t port_id = 0, level;
bool have_port = false, have_level = false;
if (!sg_attr) {
cps_api_object_it_t it;
cps_api_object_it_begin(obj, &it);
for ( ; cps_api_object_it_valid(&it); cps_api_object_it_next(&it)) {
cps_api_attr_id_t id = cps_api_object_attr_id(it.attr);
if (id == BASE_QOS_SCHEDULER_GROUP_PORT_ID) {
port_id = cps_api_object_attr_data_u32(it.attr);
have_port = true;
} else if (id == BASE_QOS_SCHEDULER_GROUP_LEVEL) {
level = cps_api_object_attr_data_u32(it.attr);
have_level = true;
}
}
}
if (!sg_attr && !have_port) {
EV_LOG_TRACE(ev_log_t_QOS, ev_log_s_MINOR, "NAS-QOS",
"Invalid input attributes for reading");
return NAS_QOS_E_MISSING_KEY;
}
uint_t switch_id = 0;
nas_obj_id_t scheduler_group_id = (sg_attr? cps_api_object_attr_data_u64(sg_attr): 0);
EV_LOG_TRACE(ev_log_t_QOS, 3, "NAS-QOS", "Read switch id %u, scheduler_group id 0x%016lX\n",
switch_id, scheduler_group_id);
std_mutex_simple_lock_guard p_m(&scheduler_group_mutex);
if (have_port) {
nas_qos_port_scheduler_group_init(port_id);
}
nas_qos_switch * p_switch = nas_qos_get_switch(switch_id);
if (p_switch == NULL)
return NAS_QOS_E_FAIL;
std::vector<nas_qos_scheduler_group *> sg_list;
if (sg_attr) {
nas_qos_scheduler_group *scheduler_group = p_switch->get_scheduler_group(scheduler_group_id);
if (scheduler_group == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, ev_log_s_MINOR, "NAS-QOS",
"Could not find scheduler group with ID %llu", scheduler_group_id);
return NAS_QOS_E_FAIL;
}
sg_list.push_back(scheduler_group);
} else {
int match_level = have_level ? (int)level : -1;
p_switch->get_port_scheduler_groups(port_id, match_level, sg_list);
}
/* fill in data */
cps_api_object_t ret_obj;
for (auto scheduler_group: sg_list) {
ret_obj = cps_api_object_list_create_obj_and_append (param->list);
if (ret_obj == NULL) {
EV_LOG_TRACE(ev_log_t_QOS, ev_log_s_MINOR, "NAS-QOS",
"Failed to create cps object");
return cps_api_ret_code_ERR;
}
cps_api_key_from_attr_with_qual(cps_api_object_key(ret_obj),BASE_QOS_SCHEDULER_GROUP_OBJ,
cps_api_qualifier_TARGET);
cps_api_set_key_data(ret_obj, BASE_QOS_SCHEDULER_GROUP_SWITCH_ID,
cps_api_object_ATTR_T_U32,
&switch_id, sizeof(uint32_t));
scheduler_group_id = scheduler_group->get_scheduler_group_id();
cps_api_set_key_data(ret_obj, BASE_QOS_SCHEDULER_GROUP_ID,
cps_api_object_ATTR_T_U64,
&scheduler_group_id, sizeof(uint64_t));
cps_api_object_attr_add_u32(ret_obj,
BASE_QOS_SCHEDULER_GROUP_CHILD_COUNT,
scheduler_group->get_child_count());
for (uint32_t idx = 0; idx < scheduler_group->get_child_count(); idx++) {
cps_api_object_attr_add_u64(ret_obj,
BASE_QOS_SCHEDULER_GROUP_CHILD_LIST,
scheduler_group->get_child_id(idx));
}
cps_api_object_attr_add_u32(ret_obj, BASE_QOS_SCHEDULER_GROUP_PORT_ID,
scheduler_group->get_port_id());
cps_api_object_attr_add_u32(ret_obj, BASE_QOS_SCHEDULER_GROUP_LEVEL,
scheduler_group->get_level());
cps_api_object_attr_add_u64(ret_obj, BASE_QOS_SCHEDULER_GROUP_SCHEDULER_PROFILE_ID,
scheduler_group->get_scheduler_profile_id());
}
return cps_api_ret_code_OK;
}