u32 rtl8821cu_inirp_init(PADAPTER padapter)
{
u8 i, status;
struct recv_buf *precvbuf;
struct dvobj_priv *pdev = adapter_to_dvobj(padapter);
struct intf_hdl *pintfhdl = &padapter->iopriv.intf;
struct recv_priv *precvpriv = &(padapter->recvpriv);
u32(*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
#ifdef CONFIG_USB_INTERRUPT_IN_PIPE
HAL_DATA_TYPE *pHalData = GET_HAL_DATA(padapter);
u32(*_read_interrupt)(struct intf_hdl *pintfhdl, u32 addr);
#endif
_read_port = pintfhdl->io_ops._read_port;
status = _SUCCESS;
precvpriv->ff_hwaddr = RECV_BULK_IN_ADDR;
/* issue Rx irp to receive data */
precvbuf = (struct recv_buf *)precvpriv->precv_buf;
for (i = 0; i < NR_RECVBUFF; i++) {
if (_read_port(pintfhdl, precvpriv->ff_hwaddr, 0, (u8 *)precvbuf) == _FALSE) {
status = _FAIL;
goto exit;
}
precvbuf++;
precvpriv->free_recv_buf_queue_cnt--;
}
#ifdef CONFIG_USB_INTERRUPT_IN_PIPE
if (pdev->RtInPipe[REALTEK_USB_IN_INT_EP_IDX] != 0x05) {
status = _FAIL;
/* RTW_INFO("%s =>Warning !! Have not USB Int-IN pipe, RtIntInPipe(%d)!!!\n", __func__, pdev->RtInPipe[REALTEK_USB_IN_INT_EP_IDX]); */
RTW_INFO("%s =>Warning !! Have not USB Int-IN pipe, RtIntInPipe(%d)!!!\n", __func__, pdev->RtInPipe[REALTEK_USB_IN_INT_EP_IDX]);
goto exit;
}
_read_interrupt = pintfhdl->io_ops._read_interrupt;
if (_read_interrupt(pintfhdl, RECV_INT_IN_ADDR) == _FALSE) {
status = _FAIL;
}
#endif
exit:
return status;
}
void r8712_read_port(struct _adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
{
struct io_queue *pio_queue = adapter->pio_queue;
struct intf_hdl *pintfhdl = &(pio_queue->intf);
u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt,
u8 *pmem);
if (adapter->bDriverStopped || adapter->bSurpriseRemoved)
return;
_read_port = pintfhdl->io_ops._read_port;
_read_port(pintfhdl, addr, cnt, pmem);
}
unsigned int rtl8195au_inirp_init(_adapter * padapter)
{
u8 i;
struct recv_buf *precvbuf;
uint status;
struct recv_priv *precvpriv = &(padapter->recvpriv);
u32 (*_read_port)(_adapter * padapter, u32 addr, u32 cnt, u8 *pmem);
#ifdef CONFIG_USB_INTERRUPT_IN_PIPE
u32 (*_read_interrupt)(_adapter * padapter, u32 addr);
#endif
_func_enter_;
_read_port = padapter->io_ops._read_port;
status = _SUCCESS;
RT_TRACE(_module_hci_hal_init_c_,_drv_info_,("===> usb_inirp_init \n"));
//issue Rx irp to receive data
precvbuf = (struct recv_buf *)precvpriv->precv_buf;
for(i=0; i<NR_RECVBUFF; i++)
{
if(_read_port(padapter, USB_READ_ADD, 0, (unsigned char *)precvbuf) == _FALSE )
{
RT_TRACE(_module_hci_hal_init_c_,_drv_err_,("usb_rx_init: usb_read_port error \n"));
status = _FAIL;
goto exit;
}
precvbuf++;
precvpriv->free_recv_buf_queue_cnt--;
}
#ifdef CONFIG_USB_INTERRUPT_IN_PIPE
_read_interrupt = padapter->io_ops._read_interrupt;
if(_read_interrupt(padapter, RECV_INT_IN_ADDR) == _FALSE )
{
RT_TRACE(_module_hci_hal_init_c_,_drv_err_,("usb_rx_init: usb_read_interrupt error \n"));
status = _FAIL;
}
#endif
exit:
RT_TRACE(_module_hci_hal_init_c_,_drv_info_,("<=== usb_inirp_init \n"));
_func_exit_;
return status;
}
void read_port(_adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
{
struct io_queue *pio_queue = (struct io_queue*)adapter->pio_queue;
struct intf_hdl *pintfhdl = &(pio_queue->intf);
u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
_func_enter_;
if ((adapter->bDriverStopped ==_TRUE) || (adapter->bSurpriseRemoved == _TRUE)) {
RT_TRACE(_module_rtl871x_io_c_, _drv_info_, ("read_port:bDriverStopped(%d) OR bSurpriseRemoved(%d)", adapter->bDriverStopped, adapter->bSurpriseRemoved));
return;
}
_read_port = pintfhdl->io_ops._read_port;
_read_port(pintfhdl, addr, cnt, pmem);
_func_exit_;
}
void _rtw_read_port(struct adapter *adapter, u32 addr, u32 cnt, u8 *pmem)
{
u32 (*_read_port)(struct intf_hdl *pintfhdl, u32 addr, u32 cnt, u8 *pmem);
struct io_priv *pio_priv = &adapter->iopriv;
struct intf_hdl *pintfhdl = &(pio_priv->intf);
_func_enter_;
if (adapter->bDriverStopped || adapter->bSurpriseRemoved) {
RT_TRACE(_module_rtl871x_io_c_, _drv_info_,
("rtw_read_port:bDriverStopped(%d) OR bSurpriseRemoved(%d)",
adapter->bDriverStopped, adapter->bSurpriseRemoved));
return;
}
_read_port = pintfhdl->io_ops._read_port;
_read_port(pintfhdl, addr, cnt, pmem);
_func_exit_;
}
// ------------------------------------------------------------------------------
// Read from Serial
// ------------------------------------------------------------------------------
int
Serial_Port::
read_message(mavlink_message_t &message)
{
uint8_t cp;
mavlink_status_t status;
uint8_t msgReceived = false;
// --------------------------------------------------------------------------
// READ FROM PORT
// --------------------------------------------------------------------------
// this function locks the port during read
int result = _read_port(cp);
// --------------------------------------------------------------------------
// PARSE MESSAGE
// --------------------------------------------------------------------------
if (result > 0)
{
// the parsing
//printf("Wert von cp %i", cp);
//printf("message payload %i \n", message);
//printf("\n Adresse von message/&message %i \n", &message);
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status);
// check for dropped packets
if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug )
{
printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
unsigned char v=cp;
fprintf(stderr,"%02x ", v);
}
lastStatus = status;
}
// Couldn't read from port
else
{
fprintf(stderr, "ERROR: Could not read from fd %d\n", fd);
}
// --------------------------------------------------------------------------
// DEBUGGING REPORTS
// --------------------------------------------------------------------------
if(msgReceived && debug)
{
// Report info
printf("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
fprintf(stderr,"Received serial data: ");
unsigned int i;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// check message is write length
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message);
// message length error
if (messageLength > MAVLINK_MAX_PACKET_LEN)
{
fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
}
// print out the buffer
else
{
for (i=0; i<messageLength; i++)
{
unsigned char v=buffer[i];
fprintf(stderr,"%02x ", v);
}
fprintf(stderr,"\n");
}
}
// Done!
return msgReceived;
}
// ------------------------------------------------------------------------------
// Read from Serial
// ------------------------------------------------------------------------------
int read_message(mavlink_message_t *message)
{
uint8_t cp;
mavlink_status_t status;
uint8_t msgReceived = 0;
/*printf("\n Wert cp %u \n", cp);*/
/*printf("\n Adresse von cp/ &cp %u \n", &cp);*/
// --------------------------------------------------------------------------
// READ FROM PORT
// --------------------------------------------------------------------------
// this function locks the port during read
int result = _read_port(&cp);
// --------------------------------------------------------------------------
// PARSE MESSAGE
// --------------------------------------------------------------------------
if (result > 0)
{
// the parsing
/*printf("cp WERT: %i \n", cp);*/
/*printf("Adresse von message: %i \n", message);*/
/*printf("Adresse des Pointers &message: %i \n", &message);*/
/*printf("Payload of message/ *message: %i \n", *message);*/
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, message, &status);
// check for dropped packets
if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug )
{
printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
unsigned char v=cp;
fprintf(stderr,"%02x ", v);
}
lastStatus = status;
}
// Couldn't read from port
else
{
fprintf(stderr, "ERROR: Could not read from fd %d\n", fd);
}
// --------------------------------------------------------------------------
// DEBUGGING REPORTS
// --------------------------------------------------------------------------
if(msgReceived && debug)
{
fprintf(stderr,"Received serial data: ");
unsigned int i;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// check message is write length
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, message);
// message length error
if (messageLength > MAVLINK_MAX_PACKET_LEN)
{
fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
}
// print out the buffer
else
{
for (i=0; i<messageLength; i++)
{
unsigned char v=buffer[i];
fprintf(stderr,"%02x ", v);
}
fprintf(stderr,"\n");
}
}
// Done!
return msgReceived;
}
//Function that reads out the port and stores it into a buffer of type mavlink_message_t
int read_message(mavlink_message_t *message)
{
// Struct of status of the reading/writing process. Defined in mavlink_types.h. For development/debugging purposes
mavlink_status_t status;
// If the data read out from the port is a whole mavlink message package msgReceived will be set to 1
uint8_t msgReceived = 0;
// Initialize a buffer of 64 byte (array of 64 uint8_t). Bytes read out from the Serial Port are stored into cp
uint8_t cp[1000];
// This function locks the port during read and reads out the port (nonblocking, up to sizeof(cp) bytes).
// Result is the number of bytes read
int result = _read_port(cp);
// Counter of the for loop needs to be declared outside of for loop. for(int i = 0; i < result; i++) results in a compiler error
int i;
// If _read_port(cp) reads out something from the serial port the bytes will be processed in the following
if (result > 0)
{
// mavlink_parse_char parses only chars. Therefore each read byte needs to be parsed individually
for(i = 0; i < result; i++)
{
// Debug purposes
if(debug == 1)
{
printf("cp: %02x \n ",cp[i]);
}
// Parsing one byte on Channel MAVLINK_COMM_1 (1) and storing the current status into mavlink_status_t status
// The parsed bytes are stored in an internal buffer on channel MAVLINK_COMM_1
// If the last byte arrived completes the message the message is copied from the internal buffer to mavlink_message_t message
// msgReceived will be set to 1 if the parsed byte completes the message. 0 otherwise
msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp[i], message, &status);
// If a mavlink message is received (all the bytes needed for a message processed by mavlink_parse_char()) handle the message by it's ID
if(msgReceived)
{
//Analysis values
all_counter += msgReceived;
/*printf("Allcounter %i \n", *all_counter);*/
werte = message->msgid;
liste[counter] = werte;
counter++;
/*printf("MSG RECEIVED and with msgid %i \n", *werte);*/
// Handle Message ID. Switching to the case depending on the msg.id from the read msg.
// First the message gets decoded by the function in the corresponding header in C_library/common/foobar.h. It is then stored (memcpy) in current_messages.msg_type
switch (message->msgid)
{
case MAVLINK_MSG_ID_GLOBAL_POSITION_INT:
{
// Decoding the message (consisting of a header and a payload with the actual content) to the corresponding Mavlink Message
mavlink_msg_global_position_int_decode(message,&(messages.global_position_int));
// Starting a timer (analysis purposes)
if (all_counter == 1){
t_px_start = messages.global_position_int.time_boot_ms/1e3;
t_last = t_px_start;
}
t_px_stop = messages.global_position_int.time_boot_ms/1e3;
printf("Sample: %f \n", t_px_stop - t_last);
t_last = t_px_stop;
/*printf("TIME: \f \n", messages.global_position_int.time_boot_ms/1);*/
break;
}
default:
{
break;
}
} // end: switch msgid
// Debug
if(status.buffer_overrun == 1 | status.parse_error == 1 | status.packet_rx_drop_count == 1)
{
printf("Buffer overruns: %i, Parse Errors: %i , Packets dropped: %i \n", status.buffer_overrun, status.parse_error, status.packet_rx_drop_count);
}
// check for dropped packets
if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug )
{
printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count);
packets_dropped++;
}
else if (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count)
{
packets_dropped++;
}
else
{
}
lastStatus = status;
}
}
}
// Couldn't read from port (nothing on the serial port, if reading frequency is higher than transmitting frequency)
else
{
printf("COULDNT READ FROM PORT");
}
// --------------------------------------------------------------------------
// DEBUGGING REPORTS
// --------------------------------------------------------------------------
if(msgReceived && debug)
{
// Printing the message read (in hexadecimal)
fprintf(stderr,"Received serial data: ");
unsigned int i;
uint8_t buffer[MAVLINK_MAX_PACKET_LEN];
// check message is write length
unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, message);
// message length error
if (messageLength > MAVLINK_MAX_PACKET_LEN)
{
fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n");
}
// print out the buffer
else
{
for (i=0; i<messageLength; i++)
{
unsigned char v=buffer[i];
fprintf(stderr,"%02x ", v);
}
fprintf(stderr,"\n");
}
}
// Some analysis variables for run.c
// Done!
return msgReceived;
}
