391 lines
9.1 KiB
C++
391 lines
9.1 KiB
C++
/*
|
|
* AesaStream.cpp
|
|
*
|
|
*/
|
|
|
|
|
|
#include "AesaStream.h"
|
|
|
|
#include "grifo_aesa_fw_if.h"
|
|
|
|
#include <string.h>
|
|
|
|
|
|
#define AESA_TX_MESSAGE_MAX_SIZE (8*1024) /*2048*/
|
|
#define AESA_RX_MESSAGE_MAX_SIZE 4096
|
|
|
|
static uint8_t aesa_response_msg[AESA_RX_MESSAGE_MAX_SIZE]; //## attribute aesa_response_msg
|
|
|
|
|
|
static int aesa_decode_rx_message_id(const char** msg_name, const void* src, unsigned int /*size*/)
|
|
{
|
|
const uint8_t* raw_data=reinterpret_cast<const uint8_t*>(src);
|
|
uint8_t response=raw_data[0];
|
|
|
|
//unsigned int expected_size=0;
|
|
const char* name="?";
|
|
|
|
*msg_name="?";
|
|
|
|
switch (response)
|
|
{
|
|
case CTI_STATUS_RSP:
|
|
name="CTISTS";
|
|
break;
|
|
case CTI_IMU_STATUS_RSP:
|
|
name="CTIIMUSTS";
|
|
break;
|
|
case CTI_EXTENDED_STATUS_RSP:
|
|
name="CTIEXTSTS";
|
|
break;
|
|
case CTI_ETI_READBACK_RSP:
|
|
name="CTIETIRB";
|
|
break;
|
|
case CTI_INIT_RESPONSE_RSP:
|
|
name="CTIINIRES";
|
|
break;
|
|
case CTI_INIT_STATUS_RSP:
|
|
name="CTIINISTS";
|
|
break;
|
|
case AESA_STATUS_RSP:
|
|
name="AESASTS";
|
|
break;
|
|
case AESA_FIRMWARE_STATUS_RSP:
|
|
name="AESAFWSTS";
|
|
//expected_size=sizeof(aesa_firmware_status_t);
|
|
break;
|
|
case AESA_BROADCAST_STATUS_RSP:
|
|
name="AESABRCSTS";
|
|
//expected_size=sizeof(aesa_broadcast_status_t);
|
|
break;
|
|
case RFIF_STATUS_RSP:
|
|
name="RFIFSTS";
|
|
break;
|
|
case RFIF_FIRMWARE_STATUS_RSP:
|
|
name="RFIFFWSTS";
|
|
break;
|
|
case RFIF_TIME_SYNC_STATUS_RSP:
|
|
name="RFIFTSSTS";
|
|
break;
|
|
case RFIF_BIT_STATUS_RSP:
|
|
name="RFIFBITE";
|
|
break;
|
|
case RFIF_NVM_DATA_RSP:
|
|
name="RFIF_NVM_DATA";
|
|
break;
|
|
case RFIF_ETI_READ_BACK_RSP:
|
|
name="RFIF_ETI_READ_BACK";
|
|
break;
|
|
case PLANK_STATUS_RSP:
|
|
name="PLANKSTS";
|
|
break;
|
|
case PLANK_EXTENDED_STATUS_RSP:
|
|
name="PLANKEXTSTS";
|
|
break;
|
|
case DOWNLOAD_MAP_RSP:
|
|
name="DMAPRES";
|
|
break;
|
|
|
|
default:
|
|
return -1;
|
|
}
|
|
|
|
*msg_name=name;
|
|
return response;
|
|
}
|
|
|
|
|
|
const char* AesaDeserializer::desResponse(DesResults* res, const void* data, unsigned int max_size)
|
|
{
|
|
//int i=0;
|
|
//int error=0;
|
|
const char* last_err_str="ok";
|
|
|
|
grifo_fw::aesa_deserializer deserializer(data, max_size);
|
|
|
|
//memset(res, 0, sizeof *res);
|
|
res->msg_num=0;
|
|
|
|
for (grifo_fw::aesa_message_descriptor_t msg=deserializer.extract(); msg.len_word>0; msg=deserializer.extract())
|
|
{
|
|
memset(aesa_response_msg, 0, sizeof(aesa_response_msg));
|
|
grifo_fw::aesa_payload_swap(aesa_response_msg, msg.data, (msg.len_word-1)*4);
|
|
|
|
DesResultMsg& m=res->msg[res->msg_num];
|
|
|
|
unsigned int tag=aesa_response_msg[1];
|
|
m.area=tag>>7;
|
|
m.tag=tag & 0x7F;
|
|
|
|
//m.tag=aesa_response_msg[1];
|
|
m.id=aesa_response_msg[0];
|
|
aesa_decode_rx_message_id(&m.name, aesa_response_msg, 0);
|
|
|
|
if (msg.sts!=grifo_fw::aesa_message_descriptor_t::ok)
|
|
{
|
|
++res->errors;
|
|
}
|
|
else
|
|
{
|
|
switch(m.id)
|
|
{
|
|
case CTI_STATUS_RSP:
|
|
{
|
|
m.payload_id=1;
|
|
aesa::cti_status_t* p=reinterpret_cast<aesa::cti_status_t*>(&aesa_response_msg[2]);
|
|
m.payload.cti_sts=*p;
|
|
if (m.payload.cti_sts.byte_0.bit.crc_error)
|
|
{
|
|
++res->crc;
|
|
++res->errors;
|
|
}
|
|
#if 0
|
|
AesaStream s(&msg.data[1], CTI_STATUS_RSP_SIZE);
|
|
m.payload.cti_sts.byte_0.raw=s.pop();
|
|
m.payload.cti_sts.byte_1.raw=s.pop();
|
|
m.payload.cti_sts.byte_2.raw=s.pop();
|
|
m.payload.cti_sts.byte_3.raw=s.pop();
|
|
#endif
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
switch((int)msg.sts)
|
|
{
|
|
case grifo_fw::aesa_message_descriptor_t::ok:
|
|
++res->ok;
|
|
break;
|
|
|
|
case grifo_fw::aesa_message_descriptor_t::crc_error:
|
|
++res->crc;
|
|
m.error=2;
|
|
last_err_str="CRC";
|
|
break;
|
|
|
|
case grifo_fw::aesa_message_descriptor_t::timeout:
|
|
++res->timeout;
|
|
m.timeout=1;
|
|
m.error=1;
|
|
last_err_str="TOUT";
|
|
break;
|
|
|
|
case grifo_fw::aesa_message_descriptor_t::len_error:
|
|
m.error=3;
|
|
last_err_str="LEN";
|
|
break;
|
|
|
|
default:
|
|
m.error=4;
|
|
last_err_str="???";
|
|
break;
|
|
}
|
|
|
|
++res->msg_num;
|
|
|
|
}
|
|
|
|
return last_err_str;
|
|
}
|
|
|
|
enum { kk_som=0x5A5A5A5A, kk_eob=0xA5A5A5A5};
|
|
|
|
|
|
static int aesa_decode_tx_message_id(const char** msg_name, unsigned int id)
|
|
{
|
|
const char* name="?";
|
|
|
|
*msg_name="?";
|
|
|
|
switch (id)
|
|
{
|
|
case CTI_READ_STATUS_CMD: // 0x01
|
|
name="CTISTS";
|
|
break;
|
|
|
|
case CTI_READ_EXTENDED_STATUS_CMD: // 0x07
|
|
name="CTIEXTSTS";
|
|
break;
|
|
case CTI_INIT_LINK_1_CMD: // 0x1E
|
|
name="CTIINI1";
|
|
break;
|
|
case CTI_INIT_LINK_2_CMD: // 0x1F
|
|
name="CTIINI2";
|
|
break;
|
|
case CTI_NVM_WRITE_ENABLE_CMD: // 0x0A
|
|
name="CTINVMWE";
|
|
break;
|
|
case CTI_TIME_SYNC_CMD: // 0x1A
|
|
name="CTITSYNC";
|
|
break;
|
|
case CTI_ARRAY_ERROR_LOG_WRITE_CMD: // 0x0E
|
|
name="CTIx0E";
|
|
break;
|
|
// AESA commands
|
|
case AESA_READ_STATUS_CMD: // 0xAE
|
|
name="A-STS";
|
|
break;
|
|
case AESA_READ_FIRMWARE_STATUS_CMD: // 0xAF
|
|
name="A-FW";
|
|
break;
|
|
|
|
|
|
// RFIF commands
|
|
case RFIF_MODE_CMD: // 0x20
|
|
name="RFIFMODE";
|
|
break;
|
|
case RFIF_READ_STATUS_CMD: // 0x22
|
|
name="RFIFSTS";
|
|
break;
|
|
case RFIF_READ_BIT_STATUS_CMD: // 0x23
|
|
name="RFIFBITE";
|
|
break;
|
|
case RFIF_READ_FW_STATUS_CMD: // 0x24
|
|
name="RFIFFW";
|
|
break;
|
|
case RFIF_TIME_SYNC_CMD: // 0x25
|
|
name="...";
|
|
break;
|
|
case RFIF_NVM_READ_DATA_CMD: // 0x2B
|
|
break;
|
|
case RFIF_NVM_ERASE_DATA_CMD: // 0x28
|
|
break;
|
|
case RFIF_NVM_WRITE_DATA_CMD: // 0x29
|
|
break;
|
|
case RFIF_ETI_READ_BACK_CMD: // 0x37
|
|
name="RFIFETI";
|
|
break;
|
|
case RF_END_TO_END_TEST_CMD: // 0xA0
|
|
name="RFETE";
|
|
break;
|
|
|
|
// PLANK commands
|
|
case PLANK_TRC_INITIALISE_CMD: // 0xC1
|
|
name="TRCINIT";
|
|
break;
|
|
case PLANK_READ_STATUS_CMD: // 0xC2
|
|
name="PLANKSTS";
|
|
break;
|
|
case PLANK_IBIT_START_CMD: // 0xC3
|
|
name="CTISTS";
|
|
break;
|
|
case PLANK_VTX_CONTROL_CMD: // 0xC4
|
|
name="PLANKVTX";
|
|
break;
|
|
case PLANK_READ_EXTENDED_STATUS_CMD: // 0xC7
|
|
name="PLANKSTS+";
|
|
break;
|
|
|
|
// BEAM calculation command
|
|
case BEAM_CALCULATION_CMD: // 0x82
|
|
name="BEAM";
|
|
break;
|
|
case SAVE_BEAM_CALCULATION_CMD: // 0x86
|
|
name="BEAMSAVE";
|
|
break;
|
|
case INDEX_BEAM_CALCULATION_CMD: // 0x88
|
|
name="BEAMINDEX";
|
|
break;
|
|
|
|
// Download Map command
|
|
case DOWNLOAD_MAP_CMD: // 0xA8
|
|
name="DMAP";
|
|
break;
|
|
|
|
default:
|
|
return -1;
|
|
}
|
|
|
|
*msg_name=name;
|
|
return 0;
|
|
}
|
|
|
|
const char* AesaDeserializer::desCommand(DesResults* res, const void* data, unsigned int max_size)
|
|
{
|
|
res->msg_num=0;
|
|
|
|
const uint32_t* src=reinterpret_cast<const uint32_t*>(data);
|
|
|
|
for(unsigned int cursor=0; cursor<(max_size/4); )
|
|
{
|
|
DesResultMsg& m=res->msg[res->msg_num];
|
|
|
|
if (src[cursor]!=kk_som)
|
|
return 0;
|
|
|
|
++cursor;
|
|
|
|
const grifo_fw::aesa_tx_header_t* p=reinterpret_cast<const grifo_fw::aesa_tx_header_t*>(&src[cursor]);
|
|
|
|
unsigned int plen=p->cw.fields.payload_len;
|
|
unsigned int nres=p->cw.fields.response;
|
|
|
|
//memset(aesa_response_msg, 0, sizeof(aesa_response_msg));
|
|
//grifo_fw::aesa_payload_swap(aesa_response_msg, &src[cursor+1], plen);
|
|
const uint8_t* b=reinterpret_cast<const uint8_t*>(&src[cursor+1]);
|
|
|
|
unsigned int tag=b[2];
|
|
m.area=tag>>7;
|
|
m.tag=tag & 0x7F;
|
|
m.id=b[3];
|
|
m.payload_id=0;
|
|
aesa_decode_tx_message_id(&m.name, m.id);
|
|
|
|
switch(m.id)
|
|
{
|
|
case BEAM_CALCULATION_CMD:
|
|
{
|
|
m.payload_id=1;
|
|
|
|
//grifo_fw::aesa_payload_swap(aesa_response_msg, &src[cursor+2], BEAM_CALCULATION_CMD_SIZE);
|
|
AesaStream s(&src[cursor+1], BEAM_CALCULATION_CMD_SIZE);
|
|
s.pop();
|
|
s.pop();
|
|
m.payload_destination=s.pop();
|
|
|
|
m.payload.beam_cmd.tx_freq=s.pop16();
|
|
m.payload.beam_cmd.rx_freq=s.pop16();
|
|
|
|
m.payload.beam_cmd.tx_u=s.popuv();
|
|
m.payload.beam_cmd.tx_v=s.popuv();
|
|
m.payload.beam_cmd.rx_u=s.popuv();
|
|
m.payload.beam_cmd.rx_v=s.popuv();
|
|
|
|
m.payload.beam_cmd.tx_ctrl_state_index=s.pop16();
|
|
m.payload.beam_cmd.tx_enable_pattern_index=s.pop();
|
|
m.payload.beam_cmd.rx_ctrl_state_index=s.pop16();
|
|
m.payload.beam_cmd.rx_enable_pattern_index=s.pop();
|
|
|
|
m.payload.beam_cmd.guard_rx_u=s.popuv();
|
|
m.payload.beam_cmd.guard_rx_v=s.popuv();
|
|
|
|
m.payload.beam_cmd.guard_rx_ctrl_state_index=s.pop16();
|
|
m.payload.beam_cmd.guard_rx_enable_pattern_index=s.pop();
|
|
|
|
m.payload.beam_cmd.sync_divisor=s.pop16();
|
|
m.payload.beam_cmd.duty_ratio=s.pop();
|
|
|
|
break;
|
|
}
|
|
default:
|
|
break;
|
|
}
|
|
|
|
unsigned int wlen=(plen+3)>>2;
|
|
cursor+=(1+wlen);
|
|
|
|
|
|
m.exp_response=nres;
|
|
|
|
|
|
++res->msg_num;
|
|
if (res->msg_num>=max_msg)
|
|
return 0;
|
|
}
|
|
|
|
return "";
|
|
}
|