bool CProtocolComm::WaitResponse(Packet &packet, int timeoutMS)
{
if (m_bResend)
{
int count = RESEND_COUNT;
while (!WaitPopLast(timeoutMS, packet))
{
count--;
if (count <= 0)
{
return false;
}
// resend
int nSend = Write((char *)&m_packet, m_packet.GetPacketLen());
if (nSend > 0)
{
WARNING("resend addr:%d type:0x%02X fun:0x%02X",
m_packet.GetDestAddr(),
m_packet.GetType(),
m_packet.GetFunction());
DEBUG_HEX((char *)&m_packet, nSend);
}
else
{
ERROR("SendFrame write fail resend");
}
}
return true;
}
else
{
return WaitPopLast(timeoutMS, packet);
}
}
int init(uint32_t mboot_magic, multiboot_info_t *mboot_info) {
// Header
console_clear();
DEBUG("------------------------------------------\n");
DEBUG("Welcome to Hydrogen AMD64 Loader 0.1\n");
DEBUG("Copyright (c) 2011 by Lukas Heidemann\n");
DEBUG("------------------------------------------\n");
// Check multiboot magic
if (mboot_magic != MULTIBOOT_BOOTLOADER_MAGIC)
PANIC("Bad multiboot magic value supplied by bootloader.\n"
"Please make sure, that your bootloader implements the Multiboot specification"
" in version 1 or higher.");
// Parse boot info and relocate modules
boot_info_t *info = boot_info_parse(mboot_info);
frame_placement = memalign(boot_modules_relocate(info, (uintptr_t) &end), 0x1000);
boot_modules_map(info);
DEBUG("Boot loader name: ");
DEBUG((int8_t *) (uintptr_t) info->loader_name);
DEBUG("\nLoader arguments: ");
DEBUG((int8_t *) (uintptr_t) mboot_info->cmdline);
DEBUG("\n");
// Find the kernel module
boot_info_mod_t *kernel_mod = _boot_find_kernel(info);
if (0 == kernel_mod)
PANIC("Could not find kernel binary.\n"
"The Hydrogen AMD64 loader expects an ELF64 kernel binary to be given as a "
"module with the name '/boot/kernel.bin'. Please make sure that your kernel "
"binary is named as such and is passed by the bootloader.");
// Map system structures
memory_map_system(info);
// Load the kernel
uint16_t pflags = PAGE_FLAG_GLOBAL;
info->entry_point = binary_load_elf64((void *) (uintptr_t) kernel_mod->address, pflags);
DEBUG("Kernel Entry Point: ");
DEBUG_HEX(info->entry_point);
DEBUG("\n");
// Set beginning of free memory
info->free_mem_begin = memalign(frame_placement, 0x1000);
// Prepare long mode trampoline code
boot_trampoline_callback = info->entry_point;
// Relocate info structure to higher half
boot_info_relocate(info, MEMORY_BOOT_INFO_VADDR);
return 0;
}
/**
* Fault Handler: General Protection Fault
*
* Kernel: Panic
* User: Terminate Process
*/
void fault_gp(cpu_int_state_t *state) {
// Is in kernel?
if (state->cs == 0x8) {
console_print("PANIC: General Protection Fault in kernel at ");
console_print_hex(state->rip);
console_print(" (error code: ");
console_print_hex(state->error_code);
console_print(").\n");
while (1);
}
// TODO: Remove this debug warning
DEBUG("General Protection Fault at ");
DEBUG_HEX(state->rip);
DEBUG(" (error code: ");
DEBUG_HEX(state->error_code);
DEBUG(").\n");
// Terminate process
process_terminate(process_current->pid);
thread_switch(scheduler_next(), state);
}
/**************************************************************************************************
* @fn macSpiReadRxFifo
*
* @brief Read data from radio receive FIFO.
*
* @param pData - pointer for storing read data
* @param len - length of data in bytes
*
* @return true if an interrupt was detected during the transfer, false otherwise
**************************************************************************************************
*/
bool mrfiSpiReadRxFifo(uint8_t * pData, uint8_t len)
{
bool result = spiBurstFifoAccess(RXFIFO | BURST_BIT | READ_BIT, pData, len);
#ifdef DEBUG_SPI
DEBUG("RX:");
while(len-- != 0)
{
DEBUG(" ");
DEBUG_HEX(*pData++);
}
DEBUG_LN("");
#endif
return result;
}
bool PacketBuffer::PacketVaild()
{
if (size() > 0)
{
//DEBUG("recv: %d", size());
//DEBUG_HEX(raw(), size());
Packet *pPacket = (Packet *)raw();
// check length
if (size() >= Packet::PACKET_MINLEN &&
size() >= pPacket->GetPacketLen())
{
// check checksum and packet end
WORD wCRC = __le16_to_cpu(*(WORD *)&(pPacket->GetData()[pPacket->GetDataLen()]));
WORD wCalCRC = pPacket->CheckSUM(&pPacket->m_btAddr, pPacket->GetPacketLen()-2);
if (wCRC == wCalCRC)
{
DEBUG("recv: %d", size());
DEBUG_HEX(raw(), size());
return true;
}
ERROR("serialport packet checksum wrong calc:%04x recv:%04X", wCalCRC, wCRC);
DEBUG_HEX(raw(), size());
}
else
{
return false;
}
// throw exception
throw(Packet::EXCEPTION_PACKET);
}
else
{
return false;
}
}
/**************************************************************************************************
* @fn mrfiSpiWriteTxFifo
*
* @brief Write data to radio transmit FIFO.
*
* @param pData - pointer for storing write data
* @param len - length of data in bytes
*
* @return true if an interrupt was detected during the transfer, false otherwise
**************************************************************************************************
*/
bool mrfiSpiWriteTxFifo(uint8_t * pData, uint8_t len)
{
#ifdef DEBUG_SPI
uint8_t len2 = len;
uint8_t * p = pData;
DEBUG("TX:");
while(len2-- != 0)
{
DEBUG(" ");
DEBUG_HEX(*p++);
}
DEBUG_LN("");
#endif
return spiBurstFifoAccess(TXFIFO | BURST_BIT, pData, len);
}
/**
* Flushes all incoming data
*/
void flush()
{
DEBUG("Flush()");
while (serial->available() > 0)
{
char c = serial->read();
DEBUG_HEX(c);
if (c != '\n' && c != '\r')
{
DEBUG_1(",");
DEBUG_1(c);
}
DEBUG_1(":");
}
DEBUG_NL();
}
bool CProtocolComm::SendFrame(BYTE btSrcAddr, BYTE btDestAddr,
BYTE btType, BYTE btFunction, void *pData, BYTE btLen, bool bClearRecvQueue)
{
if (bClearRecvQueue)
{
//DEBUG("clear receive queue");
// clear receive queue
m_queue.Clear();
}
int nLen = m_packet.BuildPacket(btSrcAddr, btDestAddr, btType, btFunction, pData, btLen);
int nSend = Write((char *)&m_packet, nLen);
if (nSend > 0)
{
DEBUG("send:");
DEBUG_HEX((char *)&m_packet, nSend);
return true;
}
ERROR("SendFrame write fail");
return false;
}
/**
* fills the given buffer until the timeout is reached or eol char is received
*/
int fillBuffer(char* cbuf, long timeout)
{
int pos = 0;
long then = millis() + timeout;
DEBUG("Buffering: -->");
while (millis() < then && pos < (BUFFER_SIZE-1) )
{
if (serial->available() > 0)
{
//Shove received byte into buffer
char c = serial->read();
DEBUG_HEX(c);
if (c != '\n' && c != '\r')
{
DEBUG_1(",");
DEBUG_1(c);
}
DEBUG_1(":");
cbuf[pos++] = c;
if (c == '\r' || c == '\n')
{
DEBUG_1( F("<-- DONE-OK: ") );
DEBUG_1(pos);
DEBUG_NL();
return pos;
}
}
}
DEBUG("<-- DONE-FAIL!");
DEBUG_NL();
return -1;
}
/** ******************************************************************
* @brief Programs a byte array to a specified address.
*
* @note This function must be used when the device voltage range is from
* 2.7V to 3.6V and an External Vpp is present.
*
* @param Address specifies the address to be programmed.
* @param Data specifies the data to be programmed.
* @param length the number of bytes to be programmed
*
* @retval HW_NVM_OK Flash written succesfully
* @retval HW_NVM_FLASH_NOT_RDY Flash was no ready for writing at start
* @retval HW_NVM_FLASH_ERASE_FAILED Something went wrong while/after erasing flash
* @retval HW_NVM_FLASH_WRITE_FAILED Something went wrong while/after writing to flash
******************************************************************* */
hw_nvm_ret_t hw_STM32F0_FLASH_WriteBlock(intptr_t Address, uint8_t *Data, uint32_t length)
{
hw_nvm_ret_t result = HW_NVM_OK;
uint32_t i;
DEBUG_LINE("hwFlash :: waiting for flash..");
/* Wait for last operation to be completed */
if( FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT) != FLASH_COMPLETE)
{
result = HW_NVM_FLASH_NOT_RDY;
}
DEBUG_LINE("hwFlash :: unlocking flash..");
/* Unlock the FLASH */
FLASH_Unlock();
DEBUG_STRING("hwFlash :: erasing page [");
DEBUG_HEX((uint8_t *)&Address, 4);
DEBUG_LINE("]");
/* Erase FLASH and Wait for last operation to be completed */
if( FLASH_ErasePage((uint32_t)Address) != FLASH_COMPLETE )
{
result = HW_NVM_FLASH_ERASE_FAILED;
}
/* Clear all FLASH flags */
FLASH_ClearFlag( FLASH_FLAG_EOP |
FLASH_FLAG_WRPERR |
FLASH_FLAG_BSY );
FLASH->CR |= FLASH_CR_PG;
DEBUG_LINE("hwFlash :: writing to flash..");
for(i=0;i<length;i+=2){
//Copy data into flash.
if(length-i == 1){ //if last (single) byte is to be copied
((uint16_t *)Address)[i/2] = (uint16_t)Data[i] ;
} else {
((uint16_t *)Address)[i/2] = ((uint16_t *)Data)[i/2] ;
}
/* Wait for last operation to be completed */
if( FLASH_WaitForLastOperation(FLASH_ER_PRG_TIMEOUT) != FLASH_COMPLETE)
{
result = HW_NVM_FLASH_WRITE_FAILED;
}
}
/* if the program operation is completed, disable the PG Bit */
FLASH->CR &= (~FLASH_CR_PG);
DEBUG_LINE("hwFlash :: locking flash..");
/* Lock the FLASH */
FLASH_Lock();
/* Return the Program Status */
return result;
}
void RTCPInstance::incomingReportHandler1() {
unsigned char* pkt = fInBuf;
unsigned packetSize;
struct sockaddr_in fromAddress;
int typeOfPacket = PACKET_UNKNOWN_TYPE;
do {
int tcpReadStreamSocketNum = fRTCPInterface.nextTCPReadStreamSocketNum();
unsigned char tcpReadStreamChannelId = fRTCPInterface.nextTCPReadStreamChannelId();
if (!fRTCPInterface.handleRead(pkt, maxPacketSize,
packetSize, fromAddress)) {
break;
}
// Ignore the packet if it was looped-back from ourself:
if (RTCPgs()->wasLoopedBackFromUs(envir(), fromAddress)) {
// However, we still want to handle incoming RTCP packets from
// *other processes* on the same machine. To distinguish this
// case from a true loop-back, check whether we've just sent a
// packet of the same size. (This check isn't perfect, but it seems
// to be the best we can do.)
if (fHaveJustSentPacket && fLastPacketSentSize == packetSize) {
// This is a true loop-back:
fHaveJustSentPacket = False;
break; // ignore this packet
}
}
if (fIsSSMSource) {
// This packet was received via unicast. 'Reflect' it by resending
// it to the multicast group.
// NOTE: Denial-of-service attacks are possible here.
// Users of this software may wish to add their own,
// application-specific mechanism for 'authenticating' the
// validity of this packet before reflecting it.
fRTCPInterface.sendPacket(pkt, packetSize);
fHaveJustSentPacket = True;
fLastPacketSentSize = packetSize;
}
#ifdef DEBUG
fprintf(stderr, "[%p]saw incoming RTCP packet (from address %s, port %d)\n", this, our_inet_ntoa(fromAddress.sin_addr), ntohs(fromAddress.sin_port));
unsigned char* p = pkt;
for (unsigned i = 0; i < packetSize; ++i) {
if (i%4 == 0) fprintf(stderr, " ");
fprintf(stderr, "%02x", p[i]);
}
fprintf(stderr, "\n");
#endif
DEBUG_LOG(INF, "Receive RTCP packet (from address %s, port %d)",
our_inet_ntoa(fromAddress.sin_addr), ntohs(fromAddress.sin_port));
DEBUG_HEX(pkt, packetSize);
int totPacketSize = IP_UDP_HDR_SIZE + packetSize;
// Check the RTCP packet for validity:
// It must at least contain a header (4 bytes), and this header
// must be version=2, with no padding bit, and a payload type of
// SR (200) or RR (201):
if (packetSize < 4) break;
unsigned rtcpHdr = ntohl(*(unsigned*)pkt);
if ((rtcpHdr & 0xE0FE0000) != (0x80000000 | (RTCP_PT_SR<<16))) {
#ifdef DEBUG
fprintf(stderr, "rejected bad RTCP packet: header 0x%08x\n", rtcpHdr);
#endif
break;
}
// Process each of the individual RTCP 'subpackets' in (what may be)
// a compound RTCP packet.
unsigned reportSenderSSRC = 0;
Boolean packetOK = False;
while (1) {
//版本,2bit, 固定为2
//填充,1bit, 不知道干嘛的
unsigned rc = (rtcpHdr>>24)&0x1F;//该包中所含接收报告块的数目,5bit
unsigned pt = (rtcpHdr>>16)&0xFF;//包类型,8bit
unsigned length = 4*(rtcpHdr&0xFFFF); // 长度,8bit,doesn't count hdr
ADVANCE(4); // skip over the header
if (length > packetSize) break;
// Assume that each RTCP subpacket begins with a 4-byte SSRC:
if (length < 4) break; length -= 4;
reportSenderSSRC = ntohl(*(unsigned*)pkt); ADVANCE(4);
Boolean subPacketOK = False;
switch (pt) {
case RTCP_PT_SR: { // 发送报告，当前活动发送者发送、接收统计
DEBUG_LOG(INF, "RTCP_PT_SR");
if (length < 20) break; length -= 20;
// Extract the NTP timestamp, and note this:
unsigned NTPmsw = ntohl(*(unsigned*)pkt); ADVANCE(4);
unsigned NTPlsw = ntohl(*(unsigned*)pkt); ADVANCE(4);
unsigned rtpTimestamp = ntohl(*(unsigned*)pkt); ADVANCE(4);
if (fSource != NULL) {
RTPReceptionStatsDB& receptionStats
= fSource->receptionStatsDB();
receptionStats.noteIncomingSR(reportSenderSSRC,
NTPmsw, NTPlsw, rtpTimestamp);
}
ADVANCE(8); // skip over packet count, octet count
// If a 'SR handler' was set, call it now:
if (fSRHandlerTask != NULL) (*fSRHandlerTask)(fSRHandlerClientData);
// The rest of the SR is handled like a RR (so, no "break;" here)
}
case RTCP_PT_RR: { // 接收报告，非活动发送者接收统计
DEBUG_LOG(INF, "RTCP_PT_RR");
unsigned reportBlocksSize = rc*(6*4);
if (length < reportBlocksSize) break;
length -= reportBlocksSize;
if (fSink != NULL) {
// Use this information to update stats about our transmissions:
RTPTransmissionStatsDB& transmissionStats = fSink->transmissionStatsDB();
for (unsigned i = 0; i < rc; ++i) {
unsigned senderSSRC = ntohl(*(unsigned*)pkt); ADVANCE(4);
// We care only about reports about our own transmission, not others'
if (senderSSRC == fSink->SSRC()) {
unsigned lossStats = ntohl(*(unsigned*)pkt); ADVANCE(4);//丢包率
unsigned highestReceived = ntohl(*(unsigned*)pkt); ADVANCE(4);//接收到的扩展的最高序列号
unsigned jitter = ntohl(*(unsigned*)pkt); ADVANCE(4); // 抖动.RTP包到达时刻统计方差的估计值
unsigned timeLastSR = ntohl(*(unsigned*)pkt); ADVANCE(4);//上一个报文的SR(发送者报告)
unsigned timeSinceLastSR = ntohl(*(unsigned*)pkt); ADVANCE(4);//自上一个报文SR的时间,以1/65536秒为单位
transmissionStats.noteIncomingRR(reportSenderSSRC, fromAddress,
lossStats,
highestReceived, jitter,
timeLastSR, timeSinceLastSR);
DEBUG_LOG(INF, "lossStats = %d, highestReceived = %d, jitter = %d, timeLastSR = %d, timeSinceLastSR = %d",
lossStats, highestReceived, jitter, timeLastSR, timeSinceLastSR);
} else {
ADVANCE(4*5);
}
}
} else {
ADVANCE(reportBlocksSize);
}
if (pt == RTCP_PT_RR) { // i.e., we didn't fall through from 'SR'
// If a 'RR handler' was set, call it now:
// Specific RR handler:
if (fSpecificRRHandlerTable != NULL) {
netAddressBits fromAddr;
portNumBits fromPortNum;
if (tcpReadStreamSocketNum < 0) {
// Normal case: We read the RTCP packet over UDP
fromAddr = fromAddress.sin_addr.s_addr;
fromPortNum = ntohs(fromAddress.sin_port);
} else {
// Special case: We read the RTCP packet over TCP (interleaved)
// Hack: Use the TCP socket and channel id to look up the handler
fromAddr = tcpReadStreamSocketNum;
fromPortNum = tcpReadStreamChannelId;
}
Port fromPort(fromPortNum);
RRHandlerRecord* rrHandler
= (RRHandlerRecord*)(fSpecificRRHandlerTable->Lookup(fromAddr, (~0), fromPort));
if (rrHandler != NULL) {
if (rrHandler->rrHandlerTask != NULL) {
(*(rrHandler->rrHandlerTask))(rrHandler->rrHandlerClientData);
}
}
}
// General RR handler:
if (fRRHandlerTask != NULL) (*fRRHandlerTask)(fRRHandlerClientData);
}
subPacketOK = True;
typeOfPacket = PACKET_RTCP_REPORT;
break;
}
case RTCP_PT_BYE: { // 表示结束
DEBUG_LOG(INF, "RTCP_PT_BYE");
// If a 'BYE handler' was set, call it now:
TaskFunc* byeHandler = fByeHandlerTask;
if (byeHandler != NULL
&& (!fByeHandleActiveParticipantsOnly
|| (fSource != NULL
&& fSource->receptionStatsDB().lookup(reportSenderSSRC) != NULL)
|| (fSink != NULL
&& fSink->transmissionStatsDB().lookup(reportSenderSSRC) != NULL))) {
fByeHandlerTask = NULL;
// we call this only once by default
(*byeHandler)(fByeHandlerClientData);
}
// We should really check for & handle >1 SSRCs being present #####
subPacketOK = True;
typeOfPacket = PACKET_BYE;
break;
}
// Later handle SDES, APP, and compound RTCP packets #####
default:
DEBUG_LOG(INF, "UNSUPPORTED TYPE(0x%x)", pt);
subPacketOK = True;
break;
}
if (!subPacketOK) break;
// need to check for (& handle) SSRC collision! #####
DEBUG_LOG(INF, "validated RTCP subpacket (type %d): %d, %d, %d, 0x%08x",
typeOfPacket, rc, pt, length, reportSenderSSRC);
// Skip over any remaining bytes in this subpacket:
ADVANCE(length);
// Check whether another RTCP 'subpacket' follows:
if (packetSize == 0) {
packetOK = True;
break;
} else if (packetSize < 4) {
#ifdef DEBUG
fprintf(stderr, "extraneous %d bytes at end of RTCP packet!\n", packetSize);
#endif
DEBUG_LOG(INF, "extraneous %d bytes at end of RTCP packet!", packetSize);
break;
}
rtcpHdr = ntohl(*(unsigned*)pkt);
if ((rtcpHdr & 0xC0000000) != 0x80000000) {
#ifdef DEBUG
fprintf(stderr, "bad RTCP subpacket: header 0x%08x\n", rtcpHdr);
#endif
DEBUG_LOG(INF, "bad RTCP subpacket: header 0x%08x", rtcpHdr);
break;
}
}
if (!packetOK) {
#ifdef DEBUG
fprintf(stderr, "rejected bad RTCP subpacket: header 0x%08x\n", rtcpHdr);
#endif
break;
} else {
#ifdef DEBUG
fprintf(stderr, "validated entire RTCP packet\n");
#endif
}
onReceive(typeOfPacket, totPacketSize, reportSenderSSRC);
} while (0);
}
