int acmp_proc_state_resp( struct jdksavdecc_frame *cmd_frame )
{
uint8_t subtype = jdksavdecc_common_control_header_get_subtype( cmd_frame->payload, ZERO_OFFSET_IN_PAYLOAD );
uint16_t seq_id = jdksavdecc_acmpdu_get_sequence_id( cmd_frame->payload, ZERO_OFFSET_IN_PAYLOAD);
uint32_t notification_flag = 0;
inflight_plist inflight_est = NULL;
if( acmp_inflight_guard == NULL )
return -1;
inflight_est = search_node_inflight_from_dblist( acmp_inflight_guard, seq_id, subtype ); // found?
if( NULL != inflight_est )
{
notification_flag = inflight_est->host_tx.inflight_frame.notification_flag;
acmp_callback( notification_flag, cmd_frame->payload, cmd_frame->length );
release_heap_space( &inflight_est->host_tx.inflight_frame.frame );// it must delect
delect_inflight_dblist_node( &inflight_est ); // delect acmp inflight node must delect date frame
}
else
{
return -1;
}
return 0;
}
int acmp_controller_state_machine::proc_resp(void *¬ification_id, struct jdksavdecc_frame *cmd_frame)
{
uint16_t seq_id = jdksavdecc_acmpdu_get_sequence_id(cmd_frame->payload, ETHER_HDR_SIZE);
uint32_t notification_flag = 0;
std::vector<inflight>::iterator j =
std::find_if(inflight_cmds.begin(), inflight_cmds.end(), SeqIdComp(seq_id));
if(j != inflight_cmds.end()) // found?
{
notification_id = (*j).cmd_notification_id;
notification_flag = (*j).notification_flag();
callback(notification_id, notification_flag, cmd_frame->payload);
inflight_cmds.erase(j);
return 1;
}
return -1;
}
int acmp_controller_state_machine::tx_cmd(void *notification_id, uint32_t notification_flag, struct jdksavdecc_frame *cmd_frame, bool resend)
{
int send_frame_returned;
if(!resend)
{
uint16_t this_seq_id = acmp_seq_id;
uint32_t msg_type = jdksavdecc_common_control_header_get_control_data(cmd_frame->payload, ETHER_HDR_SIZE);
uint32_t timeout_ms = utility->acmp_cmd_to_timeout(msg_type); // ACMP command timeout lookup
jdksavdecc_acmpdu_set_sequence_id(acmp_seq_id++, cmd_frame->payload, ETHER_HDR_SIZE);
inflight in_flight = inflight(cmd_frame,
this_seq_id,
notification_id,
notification_flag,
timeout_ms);
in_flight.start_timer();
inflight_cmds.push_back(in_flight);
}
else
{
uint16_t resend_with_seq_id = jdksavdecc_acmpdu_get_sequence_id(cmd_frame->payload, ETHER_HDR_SIZE);
std::vector<inflight>::iterator j =
std::find_if(inflight_cmds.begin(), inflight_cmds.end(), SeqIdComp(resend_with_seq_id));
if(j != inflight_cmds.end()) // found?
{
(*j).start_timer();
}
}
send_frame_returned = net_interface_ref->send_frame(cmd_frame->payload, cmd_frame->length);
if(send_frame_returned < 0)
{
log_imp_ref->post_log_msg(LOGGING_LEVEL_ERROR, "netif_send_frame error");
assert(send_frame_returned >= 0);
}
callback(notification_id, notification_flag, cmd_frame->payload);
return 0;
}
int acmp_callback( uint32_t notification_flag, uint8_t *frame, uint16_t frame_len )
{
uint32_t msg_type = jdksavdecc_common_control_header_get_control_data(frame, ZERO_OFFSET_IN_PAYLOAD);
uint16_t seq_id = jdksavdecc_acmpdu_get_sequence_id(frame, ZERO_OFFSET_IN_PAYLOAD);
uint32_t status = jdksavdecc_common_control_header_get_status(frame, ZERO_OFFSET_IN_PAYLOAD);
uint64_t end_station_entity_id;
struct jdksavdecc_eui64 stream_entity_station = jdksavdecc_common_control_header_get_stream_id( frame, ZERO_OFFSET_IN_PAYLOAD );
uint64_t end_stream_id = jdksavdecc_uint64_get(&stream_entity_station, 0);
subject_data_elem elem;
// acmp info
struct jdksavdecc_acmpdu node_acmpdu;
jdksavdecc_acmpdu_read( &node_acmpdu, frame, ZERO_OFFSET_IN_PAYLOAD, frame_len );
if((notification_flag == CMD_WITH_NOTIFICATION) &&
((msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE)))
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_talker_entity_id(frame, ZERO_OFFSET_IN_PAYLOAD);
struct jdksavdecc_eui64 listener_entity = jdksavdecc_acmpdu_get_listener_entity_id(frame, ZERO_OFFSET_IN_PAYLOAD);
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
uint64_t listener_id = jdksavdecc_uint64_get(&listener_entity, 0);
if (status == ACMP_STATUS_SUCCESS)
{
if (NULL != gp_log_imp)
gp_log_imp->log.post_log_msg( &gp_log_imp->log,
LOGGING_LEVEL_DEBUG,
" [ RESPONSE_RECEIVED: %d 0x%016llx (talker), %d, %d, %d, %s ]",
RESPONSE_RECEIVED,
end_station_entity_id,
(uint16_t)msg_type + CMD_LOOKUP,
0,
0,
acmp_cmd_status_value_to_name(status));
}
else
{
if (NULL != gp_log_imp)
gp_log_imp->log.post_log_msg( &gp_log_imp->log,
LOGGING_LEVEL_ERROR,
"[ RESPONSE_RECEIVED, 0x%016llx (talker), %s, %s, %s, %s, %d ]",
end_station_entity_id,
acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
acmp_cmd_status_value_to_name(status),
seq_id );
acmp_recv_resp_err = true;
}
// add notification of get_tx_state command to obserber
if (msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE)
{
subject_data_elem elem;
elem.listener_id = listener_id;
elem.listener_index = node_acmpdu.listener_unique_id;
elem.tarker_id = end_stream_id;
elem.tarker_index = node_acmpdu.talker_unique_id;
elem.ctrl_msg.data_type = JDKSAVDECC_SUBTYPE_ACMP;
elem.ctrl_msg.msg_type = msg_type;
elem.ctrl_msg.msg_resp_status = status;// -1 means timeout status.
elem.data_frame = NULL;// set data frame
elem.data_frame_len = 0;
set_subject_data( elem, &gconnector_subjector );
notify_observer( &gconnector_subjector );
}
}
else if((notification_flag == CMD_WITH_NOTIFICATION) &&
((msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE)))
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_listener_entity_id( frame, ZERO_OFFSET_IN_PAYLOAD );
struct jdksavdecc_eui64 _tarker_id = jdksavdecc_acmpdu_get_talker_entity_id( frame, ZERO_OFFSET_IN_PAYLOAD );
uint64_t tarker_id = jdksavdecc_uint64_get( &_tarker_id, 0 );
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
if( (status == ACMP_STATUS_SUCCESS) &&\
((msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE) || \
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE)))// udpate system descriptor connect list
{
if (NULL != gp_log_imp)
gp_log_imp->log.post_log_msg( &gp_log_imp->log,
LOGGING_LEVEL_DEBUG,
" [ RESPONSE_RECEIVED: %d 0x%016llx (listener)-0x%016llx(tarker), %d, %d, %d, %s ]",
RESPONSE_RECEIVED,
end_station_entity_id,
tarker_id,
(uint16_t)msg_type + CMD_LOOKUP,
0,
0,
acmp_cmd_status_value_to_name(status));
elem.connect_flag = (msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE)?true:false;
elem.listener_id = end_station_entity_id;
elem.listener_index = node_acmpdu.listener_unique_id;
elem.tarker_id = tarker_id;
elem.tarker_index = node_acmpdu.talker_unique_id;
elem.ctrl_msg.data_type = JDKSAVDECC_SUBTYPE_ACMP;
elem.ctrl_msg.msg_type = msg_type;
elem.ctrl_msg.msg_resp_status = status;
elem.data_frame = NULL;// set data frame
elem.data_frame_len = 0;
set_subject_data( elem, &gconnector_subjector );
notify_observer( &gconnector_subjector );
}
else if( (status == ACMP_STATUS_SUCCESS) && \
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE))
{
if (NULL != gp_log_imp)
gp_log_imp->log.post_log_msg( &gp_log_imp->log,
LOGGING_LEVEL_DEBUG,
"[ RESPONSE_RECEIVED: %d 0x%016llx (listener), %d, %d, %d, %s ]",
RESPONSE_RECEIVED,
end_station_entity_id,
(uint16_t)msg_type + CMD_LOOKUP,
0,
0,
acmp_cmd_status_value_to_name(status));
elem.listener_id = end_station_entity_id;
elem.listener_index = node_acmpdu.listener_unique_id;
elem.tarker_id = tarker_id;
elem.tarker_index = node_acmpdu.talker_unique_id;
elem.ctrl_msg.data_type = JDKSAVDECC_SUBTYPE_ACMP;
elem.ctrl_msg.msg_type = msg_type;
elem.ctrl_msg.msg_resp_status = status;
elem.data_frame = NULL;// set data frame
elem.data_frame_len = 0;
set_subject_data( elem, &gconnector_subjector );
notify_observer( &gconnector_subjector );
}
else if( status != ACMP_STATUS_SUCCESS )
{
if (NULL != gp_log_imp)
gp_log_imp->log.post_log_msg( &gp_log_imp->log,
LOGGING_LEVEL_ERROR,
"[ 0x%016llx (listener), %s, %s, %s, %s, %d ]",
end_station_entity_id,
acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
acmp_cmd_status_value_to_name(status),
seq_id);
elem.connect_flag = false;
elem.listener_id = end_station_entity_id;
elem.listener_index = node_acmpdu.listener_unique_id;
elem.tarker_id = tarker_id;
elem.tarker_index = node_acmpdu.talker_unique_id;
elem.ctrl_msg.data_type = JDKSAVDECC_SUBTYPE_ACMP;
elem.ctrl_msg.msg_type = msg_type;
elem.ctrl_msg.msg_resp_status = status;
elem.data_frame = NULL;// set data frame
elem.data_frame_len = 0;
set_subject_data( elem, &gconnector_subjector );
notify_observer( &gconnector_subjector );
acmp_recv_resp_err = true;
}
}
else if((msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE))
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_talker_entity_id(frame, ZERO_OFFSET_IN_PAYLOAD);
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
gp_log_imp->log.post_log_msg( &gp_log_imp->log,
LOGGING_LEVEL_DEBUG,
"[RESPONSE_RECEIVED, 0x%016llx (talker), %s, %s, %s, %s, %d]",
end_station_entity_id,
acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
acmp_cmd_status_value_to_name(status),
seq_id);
}
else
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_listener_entity_id(frame, ZERO_OFFSET_IN_PAYLOAD);
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
if (NULL != gp_log_imp)
gp_log_imp->log.post_log_msg( &gp_log_imp->log,
LOGGING_LEVEL_DEBUG,
"[ COMMAND_SENT, 0x%016llx listener, %s, %s, %s, %s, %d ]",
end_station_entity_id,
acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
acmp_cmd_status_value_to_name(status),
seq_id );
}
return 0;
}
ssize_t transmit_acmp_packet_network( uint8_t* frame, uint16_t frame_len, inflight_plist resend_node, bool resend ,const uint8_t dest_mac[6], bool resp, uint32_t *interval_time )
{
uint8_t sub_type = jdksavdecc_subtype_data_get_subtype(frame, ZERO_OFFSET_IN_PAYLOAD);
uint32_t msg_type = jdksavdecc_common_control_header_get_control_data(frame, ZERO_OFFSET_IN_PAYLOAD);
uint16_t seq_id = jdksavdecc_acmpdu_get_sequence_id(frame, ZERO_OFFSET_IN_PAYLOAD);
inflight_plist inflight_station = NULL;
uint32_t timeout = get_acmp_timeout( msg_type );
assert( interval_time );
*interval_time = timeout;
if( (frame_len > TRANSMIT_DATA_BUFFER_SIZE) || (frame_len <= 0) )
{
acmp_machine_debug( "udp packet( size = %d )bigger than frame buf %d or little!",
frame_len,TRANSMIT_DATA_BUFFER_SIZE );
return -1;
}
if(!resp )// not a response data
{
if( !resend )// data first send
{
inflight_station = create_inflight_dblist_new_node( &inflight_station );
if( NULL == inflight_station )
{
acmp_machine_debug("inflight station node create failed!");
return -1;
}
memset(inflight_station, 0, sizeof(inflight_list));
inflight_station->host_tx.inflight_frame.frame = allot_heap_space( frame_len, &inflight_station->host_tx.inflight_frame.frame );
if( NULL != inflight_station->host_tx.inflight_frame.frame )
{
memset(inflight_station->host_tx.inflight_frame.frame, 0, frame_len );
inflight_station->host_tx.inflight_frame.inflight_frame_len = frame_len;
memcpy( inflight_station->host_tx.inflight_frame.frame, frame, frame_len);
inflight_station->host_tx.inflight_frame.data_type = sub_type; //协议
inflight_station->host_tx.inflight_frame.seq_id = seq_id;
memcpy(&inflight_station->host_tx.inflight_frame.raw_dest, dest_mac , 6 );
if( (msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE ) || ( msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE ))
inflight_station->host_tx.inflight_frame.notification_flag = CMD_WITHOUT_NOTIFICATION;
else
inflight_station->host_tx.inflight_frame.notification_flag = CMD_WITH_NOTIFICATION;
inflight_station->host_tx.command_type = TRANSMIT_TYPE_ACMP;
inflight_station->host_tx.flags.retried = 1; // meaning send once
inflight_station->host_tx.flags.resend = false;
inflight_timer_start(timeout, inflight_station );
// 将新建的inflight命令结点插入链表结尾中
if( acmp_inflight_guard != NULL )
insert_inflight_dblist_trail( acmp_inflight_guard, inflight_station );
}
else
{
acmp_machine_debug("Err frame malloc !");
assert( NULL != inflight_station->host_tx.inflight_frame.frame );
if( NULL == inflight_station->host_tx.inflight_frame.frame )
return -1;
}
}
else
{
acmp_machine_debug( " acmp seq id = %d, subtype = %d", seq_id, sub_type );
if( resend_node != NULL ) //already search it
{
resend_node->host_tx.flags.resend = true;
resend_node->host_tx.flags.retried++ ;
inflight_timer_state_avail( timeout, resend_node );
}
else
{
acmp_machine_debug( "nothing to be resend!" );
assert(resend_node != NULL);
if( resend_node == NULL )
return -1;
}
}
}
// ready to send
uint8_t tx_frame[2048] = {0};
uint8_t ethertype[2] = {0x22, 0xf0};
int send_len = frame_len + ETHER_HDR_SIZE;
if (send_len <= 2048)
{
if (dest_mac)
{
memcpy(tx_frame+0, dest_mac, 6);
}
else
{
memcpy(tx_frame+0, net.m_default_dest_mac, 6);
}
memcpy(tx_frame+6, net.m_my_mac, 6);
memcpy(tx_frame+12, ethertype, 2);
memcpy(tx_frame + ETHER_HDR_SIZE, frame, frame_len);
controller_machine_1722_network_send(gp_controller_machine, tx_frame, send_len);
}
else
send_len = -1;
return (ssize_t)send_len;
}
int acmp_controller_state_machine::callback(void *notification_id, uint32_t notification_flag, uint8_t *frame)
{
uint32_t msg_type = jdksavdecc_common_control_header_get_control_data(frame, ETHER_HDR_SIZE);
uint16_t seq_id = jdksavdecc_acmpdu_get_sequence_id(frame, ETHER_HDR_SIZE);
uint32_t status = jdksavdecc_common_control_header_get_status(frame, ETHER_HDR_SIZE);
uint64_t end_station_entity_id;
if((notification_flag == CMD_WITH_NOTIFICATION) &&
((msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE)))
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_talker_entity_id(frame, ETHER_HDR_SIZE);
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
notification_imp_ref->post_notification_msg(RESPONSE_RECEIVED,
end_station_entity_id,
(uint16_t)msg_type + CMD_LOOKUP,
0,
0,
status,
notification_id);
if(status != ACMP_STATUS_SUCCESS)
{
log_imp_ref->post_log_msg(LOGGING_LEVEL_ERROR,
"RESPONSE_RECEIVED, 0x%llx, %s, %s, %s, %s, %d",
end_station_entity_id,
utility->acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
utility->acmp_cmd_status_value_to_name(status),
seq_id);
}
}
else if((notification_flag == CMD_WITH_NOTIFICATION) &&
((msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_CONNECT_RX_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_DISCONNECT_RX_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_RX_STATE_RESPONSE)))
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_listener_entity_id(frame, ETHER_HDR_SIZE);
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
notification_imp_ref->post_notification_msg(RESPONSE_RECEIVED,
end_station_entity_id,
(uint16_t)msg_type + CMD_LOOKUP,
0,
0,
status,
notification_id);
if(status != ACMP_STATUS_SUCCESS)
{
log_imp_ref->post_log_msg(LOGGING_LEVEL_ERROR,
"RESPONSE_RECEIVED, 0x%llx, %s, %s, %s, %s, %d",
end_station_entity_id,
utility->acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
utility->acmp_cmd_status_value_to_name(status),
seq_id);
}
}
else if((msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_STATE_RESPONSE) ||
(msg_type == JDKSAVDECC_ACMP_MESSAGE_TYPE_GET_TX_CONNECTION_RESPONSE))
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_talker_entity_id(frame, ETHER_HDR_SIZE);
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
log_imp_ref->post_log_msg(LOGGING_LEVEL_DEBUG,
"RESPONSE_RECEIVED, 0x%llx, %s, %s, %s, %s, %d",
end_station_entity_id,
utility->acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
utility->acmp_cmd_status_value_to_name(status),
seq_id);
}
else
{
struct jdksavdecc_eui64 _end_station_entity_id = jdksavdecc_acmpdu_get_listener_entity_id(frame, ETHER_HDR_SIZE);
end_station_entity_id = jdksavdecc_uint64_get(&_end_station_entity_id, 0);
log_imp_ref->post_log_msg(LOGGING_LEVEL_DEBUG,
"COMMAND_SENT, 0x%llx, %s, %s, %s, %s, %d",
end_station_entity_id,
utility->acmp_cmd_value_to_name(msg_type),
"NULL",
"NULL",
utility->acmp_cmd_status_value_to_name(status),
seq_id);
}
return 0;
}
