int PackSendPayload(uint8_t* payload, int lenPay) {
uint16_t crcPayload = crc16(payload, lenPay);
int count = 0;
uint8_t messageSend[256];
if (lenPay <= 256)
{
messageSend[count++] = 2;
messageSend[count++] = lenPay;
}
else
{
messageSend[count++] = 3;
messageSend[count++] = (uint8_t)(lenPay >> 8);
messageSend[count++] = (uint8_t)(lenPay & 0xFF);
}
memcpy(&messageSend[count], payload, lenPay);
count += lenPay;
messageSend[count++] = (uint8_t)(crcPayload >> 8);
messageSend[count++] = (uint8_t)(crcPayload & 0xFF);
messageSend[count++] = 3;
messageSend[count] = NULL;
#ifdef DEBUG
DEBUGSERIAL.print("UART package send: ");
SerialPrint(messageSend, count);
#endif // DEBUG
//Sending package
SERIALIO.write(messageSend, count);
//Returns number of send bytes
return count;
}
int
cgroups_radio_send(const void *payload, unsigned short payload_len)
{
//printk("CGROUPS RADIO: call to send\n");
//RIMESTATS_ADD(lltx);
pthread_mutex_lock(&send_lock);
//the ethernet frame being built up
ethernet_frame_t frame;
uint8_t i;
char * pay = (char *)payload;
PRINTF("SENDING ");
frame.message.length = payload_len + 2; /*extra 2 for the fcs*/
/*Can't send more than CGROUPS_RADIO_MAX_PACKET_LEN*/
if(frame.message.length > CGROUPS_RADIO_MAX_PACKET_LEN)
return -3;
for(i = 0; i < payload_len; i++)
{
frame.message.payload[i] = pay[i];
PRINTF("%c", pay[i]);
}
/*Now do the fcs calc and append*/
*(uint16_t*)(frame.message.payload +payload_len) = crc16(payload, payload_len);
//send via udp
sendViaUDP(&frame);///change it to linux version
pthread_mutex_unlock(&send_lock);
PRINTF("sending packet of length %u\n",payload_len /*+ 2*/);
return 1;
}
/* The check_crc16 function shall return 0 is the message is ignored and the
message length if the CRC is valid. Otherwise it shall return -1 and set
errno to EMBADCRC. */
int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg,
const int msg_length)
{
uint16_t crc_calculated;
uint16_t crc_received;
int slave = msg[0];
/* Filter on the Modbus unit identifier (slave) in RTU mode */
if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS) {
/* Ignores the request (not for me) */
if (ctx->debug) {
printf("Request for slave %d ignored (not %d)\n", slave, ctx->slave);
}
return 0;
}
crc_calculated = crc16(msg, msg_length - 2);
crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
/* Check CRC of msg */
if (crc_calculated == crc_received) {
return msg_length;
} else {
if (ctx->debug) {
fprintf(stderr, "ERROR CRC received %0X != CRC calculated %0X\n",
crc_received, crc_calculated);
}
if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
_modbus_rtu_flush(ctx);
}
errno = EMBBADCRC;
return -1;
}
}
/* The check_crc16 function shall return the message length if the CRC is
valid. Otherwise it shall return -1 and set errno to EMBADCRC. */
int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg,
const int msg_length)
{
uint16_t crc_calculated;
uint16_t crc_received;
crc_calculated = crc16(msg, msg_length - 2);
crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
/* Check CRC of msg */
if (crc_calculated == crc_received) {
return msg_length;
} else {
if (ctx->debug) {
fprintf(stderr, "ERROR CRC received %0X != CRC calculated %0X\n",
crc_received, crc_calculated);
}
if (ctx->error_recovery) {
_modbus_rtu_flush(ctx);
}
errno = EMBBADCRC;
return -1;
}
}
void PacketInterface::processData(QByteArray &data)
{
unsigned char rx_data;
const int rx_timeout = 50;
for(int i = 0;i < data.length();i++) {
rx_data = data[i];
switch (mRxState) {
case 0:
if (rx_data == 2) {
mRxState += 2;
mRxTimer = rx_timeout;
mRxDataPtr = 0;
mPayloadLength = 0;
} else if (rx_data == 3) {
mRxState++;
mRxTimer = rx_timeout;
mRxDataPtr = 0;
mPayloadLength = 0;
} else {
mRxState = 0;
}
break;
case 1:
mPayloadLength = (unsigned int)rx_data << 8;
mRxState++;
mRxTimer = rx_timeout;
break;
case 2:
mPayloadLength |= (unsigned int)rx_data;
if (mPayloadLength <= mMaxBufferLen) {
mRxState++;
mRxTimer = rx_timeout;
} else {
mRxState = 0;
}
break;
case 3:
mRxBuffer[mRxDataPtr++] = rx_data;
if (mRxDataPtr == mPayloadLength) {
mRxState++;
}
mRxTimer = rx_timeout;
break;
case 4:
mCrcHigh = rx_data;
mRxState++;
mRxTimer = rx_timeout;
break;
case 5:
mCrcLow = rx_data;
mRxState++;
mRxTimer = rx_timeout;
break;
case 6:
if (rx_data == 3) {
if (crc16(mRxBuffer, mPayloadLength) ==
((unsigned short)mCrcHigh << 8 | (unsigned short)mCrcLow)) {
// Packet received!
processPacket(mRxBuffer, mPayloadLength);
}
}
mRxState = 0;
break;
default:
mRxState = 0;
break;
}
}
}
void PacketInterface::firmwareUploadUpdate(bool isTimeout)
{
if (!mFirmwareIsUploading) {
return;
}
const int app_packet_size = 200;
const int retries = 5;
const int timeout = 250;
if (mFirmwareState == 0) {
mFirmwareUploadStatus = "Buffer Erase";
if (isTimeout) {
// Erase timed out, abort.
mFirmwareIsUploading = false;
mFimwarePtr = 0;
mFirmwareUploadStatus = "Buffer Erase Timeout";
} else {
mFirmwareState++;
mFirmwareRetries = retries;
mFirmwareTimer = timeout;
firmwareUploadUpdate(true);
}
} else if (mFirmwareState == 1) {
mFirmwareUploadStatus = "CRC/Size Write";
if (isTimeout) {
if (mFirmwareRetries > 0) {
mFirmwareRetries--;
mFirmwareTimer = timeout;
} else {
mFirmwareIsUploading = false;
mFimwarePtr = 0;
mFirmwareState = 0;
mFirmwareUploadStatus = "CRC/Size Write Timeout";
return;
}
quint16 crc = crc16((const unsigned char*)mNewFirmware.constData(), mNewFirmware.size());
qint32 send_index = 0;
mSendBuffer[send_index++] = COMM_WRITE_NEW_APP_DATA;
utility::buffer_append_uint32(mSendBuffer, 0, &send_index);
utility::buffer_append_uint32(mSendBuffer, mNewFirmware.size(), &send_index);
utility::buffer_append_uint16(mSendBuffer, crc, &send_index);
sendPacket(mSendBuffer, send_index);
} else {
mFirmwareState++;
mFirmwareRetries = retries;
mFirmwareTimer = timeout;
firmwareUploadUpdate(true);
}
} else if (mFirmwareState == 2) {
mFirmwareUploadStatus = "FW Data Write";
if (isTimeout) {
if (mFirmwareRetries > 0) {
mFirmwareRetries--;
mFirmwareTimer = timeout;
} else {
mFirmwareIsUploading = false;
mFimwarePtr = 0;
mFirmwareState = 0;
mFirmwareUploadStatus = "FW Data Write Timeout";
return;
}
int fw_size_left = mNewFirmware.size() - mFimwarePtr;
int send_size = fw_size_left > app_packet_size ? app_packet_size : fw_size_left;
qint32 send_index = 0;
mSendBuffer[send_index++] = COMM_WRITE_NEW_APP_DATA;
utility::buffer_append_uint32(mSendBuffer, mFimwarePtr + 6, &send_index);
memcpy(mSendBuffer + send_index, mNewFirmware.constData() + mFimwarePtr, send_size);
send_index += send_size;
sendPacket(mSendBuffer, send_index);
} else {
mFirmwareRetries = retries;
mFirmwareTimer = timeout;
mFimwarePtr += app_packet_size;
if (mFimwarePtr >= mNewFirmware.size()) {
mFirmwareIsUploading = false;
mFimwarePtr = 0;
mFirmwareState = 0;
mFirmwareUploadStatus = "FW Upload Done";
// Upload done. Enter bootloader!
QByteArray buffer;
buffer.append((char)COMM_JUMP_TO_BOOTLOADER);
sendPacket(buffer);
} else {
firmwareUploadUpdate(true);
}
}
}
}
static bool check_crc16( uint8_t const message[], size_t crc_pos ) {
uint16_t const calcCRC16 = crc16(message,crc_pos);
return ((uint8_t)(calcCRC16 & 0x00FF) == message[crc_pos+1]) && ((uint8_t)(calcCRC16 >> 8) == message[crc_pos]);
}
/**
* batadv_bla_add_claim - Adds a claim in the claim hash
* @bat_priv: the bat priv with all the soft interface information
* @mac: the mac address of the claim
* @vid: the VLAN ID of the frame
* @backbone_gw: the backbone gateway which claims it
*/
static void batadv_bla_add_claim(struct batadv_priv *bat_priv,
const uint8_t *mac, const short vid,
struct batadv_backbone_gw *backbone_gw)
{
struct batadv_claim *claim;
struct batadv_claim search_claim;
int hash_added;
memcpy(search_claim.addr, mac, ETH_ALEN);
search_claim.vid = vid;
claim = batadv_claim_hash_find(bat_priv, &search_claim);
/* create a new claim entry if it does not exist yet. */
if (!claim) {
claim = kzalloc(sizeof(*claim), GFP_ATOMIC);
if (!claim)
return;
memcpy(claim->addr, mac, ETH_ALEN);
claim->vid = vid;
claim->lasttime = jiffies;
claim->backbone_gw = backbone_gw;
atomic_set(&claim->refcount, 2);
batadv_dbg(BATADV_DBG_BLA, bat_priv,
"bla_add_claim(): adding new entry %pM, vid %d to hash ...\n",
mac, vid);
hash_added = batadv_hash_add(bat_priv->bla.claim_hash,
batadv_compare_claim,
batadv_choose_claim, claim,
&claim->hash_entry);
if (unlikely(hash_added != 0)) {
/* only local changes happened. */
kfree(claim);
return;
}
} else {
claim->lasttime = jiffies;
if (claim->backbone_gw == backbone_gw)
/* no need to register a new backbone */
goto claim_free_ref;
batadv_dbg(BATADV_DBG_BLA, bat_priv,
"bla_add_claim(): changing ownership for %pM, vid %d\n",
mac, vid);
claim->backbone_gw->crc ^= crc16(0, claim->addr, ETH_ALEN);
batadv_backbone_gw_free_ref(claim->backbone_gw);
}
/* set (new) backbone gw */
atomic_inc(&backbone_gw->refcount);
claim->backbone_gw = backbone_gw;
backbone_gw->crc ^= crc16(0, claim->addr, ETH_ALEN);
backbone_gw->lasttime = jiffies;
claim_free_ref:
batadv_claim_free_ref(claim);
}
// Verify received buffer
int is_valid_packet(uint8_t* buffer, int size) {
if(send_verify_flag) {
if(size < (buffer[7] + MODBUS_PACKET_OVERHEAD)) {
//log_info("SEND VERIFY PARTIAL DETECTED, SIZE = %d, PKT_LENGTH = %d", size, buffer[7]);
return PACKET_ERROR_LENGTH_PARTIAL;
}
if(size > (buffer[7] + MODBUS_PACKET_OVERHEAD)) {
log_info("SEND VERIFY LENGTH ERROR DETECTED, SIZE = %d, PKT_LENGTH = %d, FID = %d", size, buffer[7], buffer[8]);
{
int i;
for (i = 0; i < size; ++i) {
printf("%d %d\n", i, buffer[i]);
}
}
return PACKET_ERROR_SEND_VERIFY;
}
int i = 0;
for(i = 0; i < size; i++) {
if(buffer[i] != current_request_as_byte_array[i]) {
//log_info("SEND VERIFY ERROR DETECTED, SIZE = %d, PKT_LENGTH = %d", size, buffer[7]);
return PACKET_ERROR_SEND_VERIFY;
}
}
//log_info("SEND VERIFY OK DETECTED, SIZE = %d, PKT_LENGTH = %d", size, buffer[7]);
return PACKET_SEND_VERIFY_OK;
}
// Checking Modbus device address
if(
(_modbus_serial_config_address == 0 &&
buffer[0] == _rs485_extension.slaves[master_current_slave_to_process])
||
(_modbus_serial_config_address > 0 &&
buffer[0] ==_modbus_serial_config_address)
) {
// Checking Modbus function code
if(buffer[1] != RS485_EXTENSION_MODBUS_FUNCTION_CODE) {
return PACKET_ERROR_FUNCTION_CODE;
}
// Checking current sequence number
if(buffer[2] != current_sequence_number) {
return PACKET_ERROR_SEQUENCE_NUMBER;
}
// Checking if size exceeds empty packet length
if(size == 13) {
// Checking length of payload packet
if(buffer[7] != TINKERFORGE_HEADER_LENGTH) {
return PACKET_ERROR_LENGTH;
}
// Checking the CRC16 checksum
uint16_t crc16_calculated = crc16(buffer, size-MODBUS_PACKET_FOOTER_LENGTH);
uint16_t crc16_on_packet = ((buffer[size-MODBUS_PACKET_FOOTER_LENGTH] << 8) |
(buffer[size-(MODBUS_PACKET_FOOTER_LENGTH-1)]));
if (crc16_calculated != crc16_on_packet) {
return PACKET_ERROR_CRC16;
}
return PACKET_EMPTY_OK;
}
else {
// Checking length of payload packet
if(buffer[LENGTH_INDEX_IN_MODBUS_PACKET]+MODBUS_PACKET_OVERHEAD == size) {
// Checking the CRC16 checksum
uint16_t crc16_calculated = crc16(buffer, size-MODBUS_PACKET_FOOTER_LENGTH);
uint16_t crc16_on_packet = ((buffer[size-MODBUS_PACKET_FOOTER_LENGTH] << 8) |
(buffer[size-(MODBUS_PACKET_FOOTER_LENGTH-1)]));
if (crc16_calculated != crc16_on_packet) {
return PACKET_ERROR_CRC16;
}
return PACKET_DATA_OK;
}
else if(buffer[LENGTH_INDEX_IN_MODBUS_PACKET]+MODBUS_PACKET_OVERHEAD < size) {
// Partial receive of data packet
return PACKET_ERROR_LENGTH_PARTIAL;
}
else {
return PACKET_ERROR_LENGTH;
}
}
}
else {
log_info("RS485: %u usec, WHAT?! %d", (uint32_t)(end - start), buffer[0]);
{
int i;
for (i = 0; i < size; ++i) {
printf("xx %d %d\n", i, buffer[i]);
}
}
//exit(0);
return PACKET_ERROR_ADDRESS;
}
}
void processVariableUpdate(void)
/*++
Function Description:
This function saves the designated variables in flash storage. All variables are stored
in a flash sector (4K), as specified by the STORAGE structure. The size of STORAGE can be
up to 4K - 256 maximum. If there is a update variable request, the interrupts are disabled;
since there can be no interruptions during the flash process, nor can we have any of the
variables to be stored changing. A checksum is generated to give some validity to the ROM
based variables. Then the last sector (0x7000) is erased and the variables are written in
256 byte blocks until complete. Also contained in this sector is the firmware Signature,
which must also be updated. Finally the update request is cleared if the flash update was
successful and the interrupts are restored again.
The Signature bits that are '1' can be set to '0' without erasing the sector or upsetting
the rest of the variables. This technique is used by the Bootloader so it can indicate the
completion of Firmware Update without knowing the variable definition.
There are two blocks of variables that can be updated. The 'SPECIAL_DATA' block is reserved
for manufacturing information: Serial Number, Keyboard Layout, and LED calibration values.
These manufacturing variables are rarely updated (most likely only once on the line), the
separating of these variables provides protection against accidentaly erasure of these values.
The other page 'GENERIC_STORAGE' is for generic type variables and will be updated in the
field many times.
The SPECIAL_DATA in not a flash page as they are 4K in size, this storage is accomplished by
writing into the unused space at the top of the BootBlock page starting at SPECIAL_STORAGE.
This uses the same prinpical as mention about the Signature, you can write zeros if the
values are all ones to begin with. Since this means these variables have a limited number
of times they can be updated, great care is taken not to waste and update. The data is
always checked to see if the current values already match the new values. Other check is
made to see if the current block can be correctly written over with with the new data. If
not the block is abandon and the next block on a 64 byte boundary is used.
Arguments:
NONE
Returns:
NONE
--*/
{
if ((updateStorage != 0) && (updateOneshot == 0)) { // If variable storage need updating, then...
// __disable_irq(); // Disable interrupts while flashing, take about 12ms to write the eeprom
// if (updateOneshot !=0) {__enable_irq();} // Check updateOneshot flag again, incase that when disableing the irq,then the USB come to change the updateOneshort value.
//
if ((updateStorage & SPECIAL_DATA) != 0) { // If it is a special variable request, then...
sys.crc = crc16((U8 *)&sys, sizeof(sys)-2);
if (flashProgram(MANUFACTURING_SATRT, (void *)&sys, sizeof(sys)) == 1) {
updateStorage &= ~SPECIAL_DATA; // If flash was successful, clear need to update
// updateStorage &= ~CRC_DATA;
}
}
if ((updateStorage & GENERIC_DATA) != 0) { // If it is a generic variable request, then...
ram.crc = crc16((U8 *)&ram, sizeof(ram)-2);
if (flashProgram(USER_SETTING_START, (void *)&ram, sizeof(ram)) == 1) {
updateStorage &= ~GENERIC_DATA; // If flash was successful, clear need to update
// crc.ram = crc16((U8 *)&ram, sizeof(ram));
// updateStorage |= CRC_DATA;
}
}
// if ((updateStorage & USER_LED_DATA) != 0) { // If it is a generic variable request(For LED matrix data), then...
// if (dfu_WriteEEPROM((void*)USER_SETTING_START1, (void *)&user, sizeof(user)) == CMD_SUCCESS) {
// updateStorage &= ~USER_LED_DATA; // If flash was successful, clear need to update
// crc.user = crc16((U8 *)&user, sizeof(user));
// updateStorage |= GENERIC_DATA | CRC_DATA; // Also need to save the ram.ledEffect
// updateOneshot = BUFFERING_TIME_SW; // Apply one short
// }
// }
// if ((updateStorage & CRC_DATA) != 0) {
// if (dfu_WriteEEPROM((void*)CRC16_START, (void *)&crc, sizeof(crc)) == CMD_SUCCESS) {
// updateStorage &= ~CRC_DATA; // If flash was successful, clear need to update
// }
// }
// if (deviceMode == BOOTLOADER_MODE) { // If switching to Bootloader mode, then...
//#if 0
// *(U32 *)(0x10000FF8) = 0xAAAAAAAA; // Set soft enter bootloader flag
// NVIC_SystemReset(); // Request a MCU soft reset
//#else
// dfu_EraseSector(SIGNATURE_ADDR>>12, SIGNATURE_ADDR>>12);
// NVIC_SystemReset();
// for (; ; );
// //if (dfu_EraseSector(5, 5) == CMD_SUCCESS) { // Erase the last sector (number 7)
// // dfu_InitializeBuffer(); // Initialize the flash buffer
// // flashBuffer[(SIGNATURE_ADDR&0x00FF)>>2] = DFU_SIGNATURE;
// // dfu_CopyToFlash((void *)flashBuffer, (void *)(SIGNATURE_ADDR&0xFF00), 256);
// // NVIC_SystemReset();
// //}
//#endif
// }
// __enable_irq(); // Re-enable interrupts
}
if (factoryReset == 4) {
memoryCopy((void *)&ram, (void *)&dft_generic, sizeof(ram));
requestVariableUpdate(SW_CHANGED);
forceLightingReload();
// forceLightingUpdate();
factoryReset = 0;
}
}
void comm_cc2520_send_buffer(uint8_t *data, unsigned int len) {
uint8_t send_buffer[MAX_PL_LEN];
if (len <= (MAX_PL_LEN - 1)) {
uint32_t ind = 0;
send_buffer[ind++] = MOTE_PACKET_PROCESS_SHORT_BUFFER;
memcpy(send_buffer + ind, data, len);
ind += len;
comm_cc2520_send_packet(send_buffer, ind);
} else {
unsigned int end_a = 0;
unsigned int len2 = len - (MAX_PL_LEN - 5);
for (unsigned int i = 0;i < len2;i += (MAX_PL_LEN - 2)) {
if (i > 255) {
break;
}
end_a = i + (MAX_PL_LEN - 2);
uint8_t send_len = (MAX_PL_LEN - 2);
send_buffer[0] = MOTE_PACKET_FILL_RX_BUFFER;
send_buffer[1] = i;
if ((i + (MAX_PL_LEN - 2)) <= len2) {
memcpy(send_buffer + 2, data + i, send_len);
} else {
send_len = len2 - i;
memcpy(send_buffer + 2, data + i, send_len);
}
comm_cc2520_send_packet(send_buffer, send_len + 2);
}
for (unsigned int i = end_a;i < len2;i += (MAX_PL_LEN - 3)) {
uint8_t send_len = (MAX_PL_LEN - 3);
send_buffer[0] = MOTE_PACKET_FILL_RX_BUFFER_LONG;
send_buffer[1] = i >> 8;
send_buffer[2] = i & 0xFF;
if ((i + (MAX_PL_LEN - 3)) <= len2) {
memcpy(send_buffer + 3, data + i, send_len);
} else {
send_len = len2 - i;
memcpy(send_buffer + 3, data + i, send_len);
}
comm_cc2520_send_packet(send_buffer, send_len + 3);
}
uint32_t ind = 0;
send_buffer[ind++] = MOTE_PACKET_PROCESS_RX_BUFFER;
send_buffer[ind++] = len >> 8;
send_buffer[ind++] = len & 0xFF;
unsigned short crc = crc16(data, len);
send_buffer[ind++] = (uint8_t)(crc >> 8);
send_buffer[ind++] = (uint8_t)(crc & 0xFF);
memcpy(send_buffer + 5, data + len2, len - len2);
ind += len - len2;
comm_cc2520_send_packet(send_buffer, ind);
}
}
static THD_FUNCTION(rx_thread, arg) {
(void)arg;
chRegSetThreadName("CC2520 RX");
for(;;) {
if (basicRfPacketIsReady()) {
static uint8_t buf[130];
unsigned int len = 0;
unsigned int ind = 0;
len = basicRfReceive(buf, 130, NULL);
MOTE_PACKET packet = buf[0];
led_toggle(LED_RED);
switch (packet) {
case MOTE_PACKET_FILL_RX_BUFFER:
memcpy(rx_buffer + buf[1], buf + 2, len - 2);
break;
case MOTE_PACKET_FILL_RX_BUFFER_LONG: {
int rxbuf_ind = (unsigned int)buf[1] << 8;
rxbuf_ind |= buf[2];
if (rxbuf_ind < RX_BUFFER_SIZE) {
memcpy(rx_buffer + rxbuf_ind, buf + 3, len - 3);
}
}
break;
case MOTE_PACKET_PROCESS_RX_BUFFER: {
ind = 1;
int rxbuf_len = (unsigned int)buf[ind++] << 8;
rxbuf_len |= (unsigned int)buf[ind++];
if (rxbuf_len > RX_BUFFER_SIZE) {
break;
}
uint8_t crc_high = buf[ind++];
uint8_t crc_low = buf[ind++];
memcpy(rx_buffer + rxbuf_len - (len - ind), buf + ind, len - ind);
if (crc16(rx_buffer, rxbuf_len)
== ((unsigned short) crc_high << 8
| (unsigned short) crc_low)) {
comm_usb_send_packet(rx_buffer, rxbuf_len);
}
}
break;
case MOTE_PACKET_PROCESS_SHORT_BUFFER:
comm_usb_send_packet(buf + 1, len - 1);
break;
default:
break;
}
}
chThdSleepMicroseconds(100);
}
}
//-----------------------------------------------------------------------------
//! Nazwa funkcji : RF_ExecuteOrder
//! Funkcja interpretuje i realizuje otrzymany rozkaz
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
void RF_ExecuteOrder(_RF_Command *Command)
{
TRACE_INFO("Id: %X, Ord: %X, Reg: %X, Val: %X \n\r", Command->ID, Command->Order, Command->Auxi, Command->B8.Val);
licznik_cykli=0;
_RF_Command CommandToSend;
//cz�� wsplna wszystkich odpowiedzi
CommandToSend.ID = RF_ID_Master;
CommandToSend.Auxi = Command->Auxi;
char ByteToSend=0;
switch (Command->Order)
{
//Rozkazy dla Slave ------------------------------------------------------------
case R_RegOrd : //Odczyt pojedy�czego rejestru - rozkaz
//wykonanie ------
CommandToSend.B8.Val = SymulacjaRejWej; // przepisz dane z rejestru nr: Command.Auxi (tymczasowo SymulacjaRej)
//odpowied� ------
CommandToSend.Order = R_RegRes;
CommandToSend.B8.CRC = crc16(CommandToSend.Streem,6);
ByteToSend =8;
break;
case W_RegOrd : //Zapis pojedy�czego rejestru -rozkaz
//wykonanie ------
SymulacjaRejWyj==Command->B8.Val; //Ustaw rejestr nr: Command.Auxi
//odpowied� ------
CommandToSend.Order = W_RegRes;
CommandToSend.B6.CRC = crc16(CommandToSend.Streem,4);
ByteToSend =6;
break;
/*
case RW_RegOrd : //Zapis + odczyt pojedy�czego rejestru - rozkaz
//wykonanie ------
CommandToSend.B8.Val = SymulacjaRejWej; // przepisz dane z rejestru nr: Command.Auxi (tymczasowo SymulacjaRej)
SymulacjaRejWyj==Command->B8.Val; //Ustaw rejestr nr: Command.Auxi
//odpowied� ------
CommandToSend.Order = RW_RegRes;
CommandToSend.B8.CRC = crc16(CommandToSend.Streem,6);
ByteToSend =8;
break;
*/
//Odpowiedzi rozkaz�w od slave do sterownika ------------------------------------------------------------
case R_RegRes : //Odczyt pojedy�czego rejestru - odp
//wykonanie ------
//CommandToSend.B8.Val = 0xCC; // przepisz dane z rejestru nr: Command.Auxi (tymczasowo 0xCC)
MainStage.StageI[RF_TaskTab[RF_CurrentTask].Node].Value[RF_TaskTab[RF_CurrentTask].Command.Auxi]=Command->B8.Val;
TransQuality.Mensur[RF_TaskTab[RF_CurrentTask].Node-1] |= 0x1 << TransQuality.MensurIndex; //wpis do tablicy jako�ci
break;
case W_RegRes : //Zapis pojedy�czego rejestru -odp
//wykonanie ------
if ((TransQuality.MensurIndex>32) || (TransQuality.MensurIndex<0))
{
TransQuality.MensurIndex=0;
}
if (RF_TaskTab[RF_CurrentTask].Node-1<=NumberNeurons)
{
TransQuality.Mensur[RF_TaskTab[RF_CurrentTask].Node-1] |= 0x1 << TransQuality.MensurIndex; //wpis do tablicy jako�ci
}
}
//Wy�lij odpowied�
if (ByteToSend)
{
RF_Status.EnableTX();
RF_SendData(CommandToSend.Streem, ByteToSend);
RF_Status.EnableRX();
}
}
//-----------------------------------------------------------------------------
//! Nazwa funkcji : RF_ReciveByte
//! Funkcja interpretacji otrzymanego bajtu
//-----------------------------------------------------------------------------
void RF_ReciveByte(char Res)
{
for (char i=0; i<RF_BuffSizeRX-1; i++) {RF_Rx.Buff[i]=RF_Rx.Buff[i+1];}
RF_Rx.Buff[RF_BuffSizeRX-1]=Res;
RF_Rx.ReciveBytes++;
//sprawdzenie ko�ca
if (
(RF_Rx.Buff[RF_BuffSizeRX-3]==Preambu[0]) &&
(RF_Rx.Buff[RF_BuffSizeRX-2]==Preambu[1]) &&
(RF_Rx.Buff[RF_BuffSizeRX-1]==Preambu[2]))
{
RF_Rx.ReciveBytes -=3; //zmiejszenie ilo�ci odebranych danych o preambu�e
//je�eli danych wi�cej ni� max ilo�� bajt�w w rozkazie to pr�ba dopasowania
if (RF_Rx.ReciveBytes>8)
{
pRF_Command=(_pRF_Command)&(RF_Rx.Buff[6+3]); //rzutowanie na struktur� rozkazu
if (pRF_Command->B6.CRC == crc16(pRF_Command->Streem, 4)) RF_Rx.ReciveBytes=6;
pRF_Command=(_pRF_Command)&(RF_Rx.Buff[8+3]); //rzutowanie na struktur� rozkazu
if (pRF_Command->B6.CRC == crc16(pRF_Command->Streem, 6)) RF_Rx.ReciveBytes=8;
}
pRF_Command=(_pRF_Command)&(RF_Rx.Buff[RF_BuffSizeRX-(RF_Rx.ReciveBytes+3)]); //rzutowanie na struktur� rozkazu
abcd=crc16(pRF_Command->Streem, RF_Rx.ReciveBytes-2);
if (
((RF_Rx.ReciveBytes==6) && (pRF_Command->B6.CRC == crc16(pRF_Command->Streem, RF_Rx.ReciveBytes-2))) ||
((RF_Rx.ReciveBytes==8) && (pRF_Command->B8.CRC == crc16(pRF_Command->Streem, RF_Rx.ReciveBytes-2)))
)
{
g_RF_TimeOutCounter=0; //Restart timeoutu
///Sprawdzenie zgodno�ci ID
//if (pRF_Command->ID == RF_ID)
//{
RF_ExecuteOrder(pRF_Command); //realizacja rozkazu
//}
//Testowe dla mastera
// if (pRF_Command->ID == RF_ID_Master)
// {
poprawnych++;
odb[IndWys]++;
// }
RF_Rx.ReciveBytes=0;
// TRACE_INFO("\n\r");
// for (char i=0; i<20; i++) TRACE_INFO("%X ", RF_Rx.Buff[i]);
/*
if ((RF_Rx.Buff[RF_BuffSizeRX-9]==0x45) )
{
}
else if ((RF_Rx.Buff[RF_BuffSizeRX-9]==0x35) )
{
}
*/
}else
{
RF_Rx.ReciveBytes=0;
}
if (!RF_Tx.BytesToSend) SPI_SendWord(0xCA81,Cs_RF);
}
else{
if ((( AT91C_BASE_PIOA->PIO_PDSR) & RfIntBUS)==0) SPI_SendWord(0x0000,Cs_RF);
}
}
/*顺舟转上海协议
* @concentrator 集中器地址
* @source[] 顺舟协议
* @sourceLen 顺舟协议长度
* @target[] 传出参数，转换后的上海协议
* @*targetSize 传出参数，转换后的上海协议长度
* */
void Sz2Sh(unsigned long concentrator, unsigned char source[], int sourceLen,
unsigned short shCmd, unsigned char target[], int *targetSize)
{
/*
if(sourceLen == 5) //注册包或心跳包
shCmd = 0xFF11;
else{
switch(source[0]){
case 0x11:
switch(source[1]){
case 0x1D: //终端(灯控器)发送数据格式
shCmd = 0xFF21;
break;
case 0x0A: //回路查询响应帧 和 控制指令响应帧
shCmd = 0xFF31;
break;
case 0x35: //3相电查询响应帧
shCmd = 0xFF33;
break;
}
break;
case 0x20: //集中管理器配置
switch(source[2]){
case 0x15: //读取网关本地时间响应帧
shCmd = 0xFF34;
break;
case 0x1F: //读取策略时间响应帧
shCmd = 0xFF35;
break;
case 0x19: //读取经纬度响应帧
shCmd = 0xFF36;
break;
}
break;
}
}
*/
//封装上海协议
struct shProtocal frame; //上海协议数据帧
memset(&frame, 0, sizeof(struct shProtocal));
frame.head = 0xAA;
long2bigEndian(frame.concentrator, concentrator);
frame.cmd_u.cmd = htons(shCmd);
frame.length_u.len = htonl(sourceLen);
frame.pData = source;
// printf("s:%d e:%d\n", sourceLen, frame.length_u.len);
//封装上海协议帧
target[0] = frame.head;
memcpy(target+1, frame.concentrator, 8);
memcpy(target+25, frame.cmd_u.cmdBytes, 2);
memcpy(target+30, frame.length_u.lenBytes, 4);
memcpy(target+34, frame.pData, sourceLen);
//获取CRC16校验码
frame.checkCode_u.checkCode = htons(crc16(target, 34+sourceLen));
frame.tail = 0x55;
memcpy(target+34+sourceLen, frame.checkCode_u.checkCodeBytes, 2);
target[34+sourceLen+2] = frame.tail;
*targetSize = 34+sourceLen+3;
}
bit mb_processframe()
{
uint8_t err;
uint16_t val;
uint16_t dataaddr = mb_getdataaddr();
uint16_t dataval = mb_getdataval();
switch (mb_getfunc()) {
case MB_FNCODE_RD_COIL:
err = proto_get_rw_byte(dataaddr, dataval, (uint8_t*)&val);
goto mb_check;
case MB_FNCODE_RD_DISCRETE_INPUT:
err = proto_get_ro_byte(dataaddr, dataval, (uint8_t*)&val);
goto mb_check;
case MB_FNCODE_RD_HOLDING_REGISTER:
err = proto_get_rw_word(dataaddr, dataval, &val);
goto mb_check;
case MB_FNCODE_RD_INPUT_REGISTER:
err = proto_get_ro_word(dataaddr, dataval, &val);
goto mb_check;
case MB_FNCODE_WR_SINGLE_COIL: {
switch (dataval) {
default:
err = MB_EVAL;
goto mb_check;
case 0x0000:
val = false;
break;
case 0xFF00:
val = true;
break;
}
err = proto_set_rw_byte(dataaddr, val);
} goto mb_check;
case MB_FNCODE_WR_SINGLE_REGISTER: {
val = dataval;
err = proto_set_rw_word(dataaddr, val);
} goto mb_check;
default:
err = MB_EFUNC;
break;
}
err = MB_EFUNC;
mb_check:
if (mb_isbroadcast()) {
return false;
}
mb_frame[0] = mb_addr;
if (err != MB_EOK) {
mb_frame[1] = mb_getfunc() + 0x80;
mb_frame[2] = err;
mb_framelen = 3;
}
else {
mb_frame[1] = mb_getfunc();
switch (mb_getfunc()) {
default:
err = MB_EFUNC;
goto mb_check;
case MB_FNCODE_RD_COIL:
case MB_FNCODE_RD_DISCRETE_INPUT:
mb_frame[2] = (uint8_t)dataval;
mb_frame[3] = (uint8_t)val;
mb_framelen = 4;
break;
case MB_FNCODE_RD_HOLDING_REGISTER:
case MB_FNCODE_RD_INPUT_REGISTER:
mb_frame[2] = (uint8_t)(2 * dataval);
to_uint16(&mb_frame[3]) = htons(val);
mb_framelen = 5;
break;
case MB_FNCODE_WR_SINGLE_COIL:
to_uint16(&mb_frame[2]) = htons(dataaddr);
to_uint16(&mb_frame[4]) = htons(dataval);
mb_framelen = 6;
break;
case MB_FNCODE_WR_SINGLE_REGISTER:
to_uint16(&mb_frame[2]) = htons(dataaddr);
to_uint16(&mb_frame[4]) = htons(dataval);
mb_framelen = 6;
break;
}
}
to_uint16(&mb_frame[mb_framelen]) = crc16(mb_frame, mb_framelen);
mb_framelen += 2;
return true;
}
int main(int argc, char const *argv[]) {
g_fd = 0x00;
struct response *resp;
struct timer_data *data;
uint8_t buff[8] = {0x00};
uint16_t crc = 0x00;
if (argc <= 3) {
fprintf(stderr, "host_slip <prescaler> <ocr> <st_max>\n");
return -1;
}
if (0 > (g_fd = open(SERIAL_PORT, O_RDWR | O_NOCTTY | O_SYNC))) {
fprintf(stderr, "Unable to open device [%s]\n", SERIAL_PORT);
return 0;
}
tty_attrib_conf(g_fd, BAUD_9600, 0);
tty_block_conf(g_fd, 1);
data = (struct timer_data *)buff;
data->crc16 = 0x0000;
data->prescaler = atoi(argv[1]);
data->ocr = atoi(argv[2]);
data->st_max = atoi(argv[3]);
// insert the CRC
crc = crc16(0x00, buff, sizeof(struct timer_data));
data->crc16 = crc;
// wait for the board to reset itself
sleep(4);
printf("Sending: %d/%d/%d, CRC: 0x%04x\n",
data->prescaler,
data->ocr,
data->st_max,
data->crc16);
slip_send(buff, sizeof(struct timer_data));
slip_recv(buff, sizeof(buff));
resp = (struct response *)buff;
// zero the CRC to perform verification
buff[6] = buff[0];
buff[7] = buff[1];
buff[0] = 0x00;
buff[1] = 0x00;
// we've got the calculated CRC now
crc = crc16(0x00, buff, sizeof(struct response));
// restore the CRC
buff[0] = buff[6];
buff[1] = buff[7];
printf("CRC/CRC_CALCD/STATUS: 0x%04x/0x%04x/%s\n",
resp->crc16,
crc,
(resp->status == 0x10 ? "ACK" : "NACK"));
close(g_fd);
return 0;
}
void AnalyzeRecieve(void)
{
unsigned int crc16tem;
char i,j;
int TempHighAddr;
if (CommBuf[CommIndexSlaveID]==SlaveID) // The slave ID is matched
{
crc16tem=crc16(CommBuf,CommIndex-1); // calculate CRC check, erase two CRC byte
if (crc16tem==(((unsigned int)(CommBuf[CommIndexEnd]) << 8) | CommBuf[CommIndexEnd-1]))
{ // crc16 check is OK
switch (CommBuf[CommIndexFunction])
{
case ReadMulCoilSta:
CommBuf[CommIndexSlaveID]=SlaveID; // 0
CommBuf[CommIndexFunction]=ReadMulCoilSta; // 2
CommBuf[2]=CommBuf[CommIndexNoPointLo] / 0x08; // 0x16 bit
writeadr=CommBuf[3]; //
SlaveOutputBuf[0]=0;
SlaveOutputBuf[1]=0;
for(i=0;i<8;i++,writeadr++)
SlaveOutputBuf[0]=SlaveOutputBuf[0]+(ModebusBitsDataBuff[writeadr]<<i);// send data of master force coil val
for(i=0;i<8;i++,writeadr++)
SlaveOutputBuf[1]=SlaveOutputBuf[1]+(ModebusBitsDataBuff[writeadr]<<i);
i=CommBuf[2]+3;
for(CommIndex=3;CommIndex<i;CommIndex++) // send data Reg to Master
CommBuf[CommIndex]=SlaveOutputBuf[CommIndex-3]; // hight 8 bit is first send
crc16tem=crc16(CommBuf,CommIndex); // then send low 8 bit data
CommBuf[CommIndex++]=(char)(crc16tem & 0x00ff); // send crccheck low 8 bit is front
CommBuf[CommIndex]=(char)(crc16tem>>8); // then send hight 8 bit
CommIndexEnd=CommIndex;
CommIndex=0;
for(i=0;i<=CommIndexEnd;i++)
{
// ScicRegs.SCITXBUF=CommBuf[i];
SciTxQueue[SciTxHighPoint]=CommBuf[i];
SciTxHighPoint++;
if(SciTxHighPoint==MaxDataLen)
SciTxHighPoint=0;
}
break;
case ForceSingleCoil:
// get data 0x00 or 0xff
writeadr=CommBuf[3];
if (CommBuf[4]==0xff)
{
ModebusBitsDataBuff[writeadr]=1;
}
else
{
ModebusBitsDataBuff[writeadr]=0;
}
CommBuf[CommIndexSlaveID]=SlaveID; // 0
CommBuf[CommIndexFunction]=ForceSingleCoil; // 1
CommIndex=2;
CommBuf[CommIndex++]=CommBuf[CommIndexStartAdrHi]; // 2
CommBuf[CommIndex++]=CommBuf[CommIndexStartAdrLo]; // 3
CommBuf[CommIndex++]=CommBuf[CommIndexNoPointHi]; // 4
CommBuf[CommIndex++]=CommBuf[CommIndexNoPointLo]; // 5
crc16tem=crc16(CommBuf,CommIndex); // then send low 8 bit data
CommBuf[CommIndex++]=(char)(crc16tem & 0x00ff); // send crccheck low 8 bit is front
CommBuf[CommIndex]=(char)(crc16tem>>8); // then send hight 8 bit
CommIndexEnd=CommIndex;
CommIndex=0;
for(i=0;i<=CommIndexEnd;i++)
{
// ScicRegs.SCITXBUF=CommBuf[i];
SciTxQueue[SciTxHighPoint]=CommBuf[i];
SciTxHighPoint++;
if(SciTxHighPoint==MaxDataLen)
SciTxHighPoint=0;
}
break;
case ReadHoldReg:
TempHighAddr=CommBuf[2];
writeadr=((int)CommBuf[3])+(TempHighAddr<<8);
CommBuf[CommIndexSlaveID]=SlaveID; // 0
CommBuf[CommIndexFunction]=ReadHoldReg; // 1
CommBuf[2]=CommBuf[CommIndexNoPointLo]*2; // 2 Byte Count
for (i=0;i<CommBuf[2];i++) // sim inputval for test
SlaveOutputBuf[i]=ModebusRegsDataBuff[writeadr++];
i=CommBuf[2]+3;
for(CommIndex=3;CommIndex<i;CommIndex++) // send data Reg to Master
CommBuf[CommIndex]=SlaveOutputBuf[CommIndex-3]; // hight 8 bit is first send
crc16tem=crc16(CommBuf,CommIndex); // then send low 8 bit data
CommBuf[CommIndex++]=(char)(crc16tem & 0x00ff); // send crccheck low 8 bit is front
CommBuf[CommIndex]=(char)(crc16tem>>8); // then send hight 8 bit
CommIndexEnd=CommIndex;
CommIndex=0;
for(i=0;i<=CommIndexEnd;i++)
{
// ScicRegs.SCITXBUF=CommBuf[i];
SciTxQueue[SciTxHighPoint]=CommBuf[i];
SciTxHighPoint++;
if(SciTxHighPoint==MaxDataLen)
SciTxHighPoint=0;
}
break;
case PresetMulReg:
TempHighAddr=CommBuf[2];
writeadr=((int)CommBuf[3])+(TempHighAddr<<8);
j=CommBuf[CommIndexNoPointLo]*2;
for (i=0;i<j;i++)
{
ModebusRegsDataBuff[writeadr]=CommBuf[i+7]; // get data that master send start 7th byte
writeadr++;
}
CommBuf[CommIndexSlaveID]=SlaveID; // 0
CommBuf[CommIndexFunction]=PresetMulReg; // 1
CommIndex=2;
CommBuf[CommIndex++]=CommBuf[CommIndexStartAdrHi]; // 2
CommBuf[CommIndex++]=CommBuf[CommIndexStartAdrLo]; // 3
CommBuf[CommIndex++]=CommBuf[CommIndexNoPointHi]; // 4
CommBuf[CommIndex++]=CommBuf[CommIndexNoPointLo]; // 5
crc16tem=crc16(CommBuf,CommIndex); // then send low 8 bit data
CommBuf[CommIndex++]=(char)(crc16tem & 0x00ff); // send crccheck low 8 bit is front
CommBuf[CommIndex]=(char)(crc16tem>>8); // then send hight 8 bit
CommIndexEnd=CommIndex;
CommIndex=0;
for(i=0;i<=CommIndexEnd;i++)
{
// ScicRegs.SCITXBUF=CommBuf[i];
SciTxQueue[SciTxHighPoint]=CommBuf[i];
SciTxHighPoint++;
if(SciTxHighPoint==MaxDataLen)
SciTxHighPoint=0;
}
break;
default:
for (i=0;i<=80;i++) // delay // error recieve again
asm(" NOP");
CommIndex=0;
//T0=RECIEVE;
break;
}
}
else
{
for (i=0;i<=80;i++) // delay
IPState Vantage::updateWeather()
{
int nbytes_written=0, nbytes_read=0, rc=-1;
char errstr[MAXRBUF];
char command[VANTAGE_CMD];
char response[VANTAGE_RES];
if (wakeup() == false)
return IPS_ALERT;
strncpy(command, "LOOP 1", VANTAGE_CMD);
command[6] = 0;
tcflush(PortFD, TCIOFLUSH);
DEBUGF(INDI::Logger::DBG_DEBUG, "CMD (%s)", command);
command[6] = 0xA;
if ( (rc = tty_write(PortFD, command, 7, &nbytes_written)) != TTY_OK)
{
tty_error_msg(rc, errstr, MAXRBUF);
DEBUGF(INDI::Logger::DBG_ERROR, "Loop error: %s.", errstr);
return IPS_ALERT;
}
if ( (rc = tty_read(PortFD, response, 1, VANTAGE_TIMEOUT, &nbytes_read)) != TTY_OK)
{
tty_error_msg(rc, errstr, MAXRBUF);
DEBUGF(INDI::Logger::DBG_ERROR, "Loop error: %s.", errstr);
return IPS_ALERT;
}
if (response[0] != 0x06)
{
DEBUGF(INDI::Logger::DBG_ERROR, "Expecting 0x06, received %#X", response[0]);
return IPS_ALERT;
}
if ( (rc = tty_read(PortFD, response, 99, VANTAGE_TIMEOUT, &nbytes_read)) != TTY_OK)
{
tty_error_msg(rc, errstr, MAXRBUF);
DEBUGF(INDI::Logger::DBG_ERROR, "Loop error: %s.", errstr);
return IPS_ALERT;
}
uint16_t crc = crc16(response, 99);
if (crc != 0)
{
DEBUG(INDI::Logger::DBG_ERROR, "CRC check failed.");
return IPS_ALERT;
}
uint8_t *loopData = (uint8_t *) response;
DEBUGF(INDI::Logger::DBG_DEBUG, "Packet Type (%d)", loopData[4]);
uint8_t forecastValue = loopData[89];
DEBUGF(INDI::Logger::DBG_DEBUG, "Raw Forecast (%d)", forecastValue);
switch (forecastValue)
{
// Clear
case 0x08:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Mostly Clear.");
setParameterValue("WEATHER_FORECAST", 0);
break;
case 0x06:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Partly Cloudy.");
setParameterValue("WEATHER_FORECAST", 1);
break;
case 0x02:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Mostly Cloudy.");
setParameterValue("WEATHER_FORECAST", 2);
break;
case 0x03:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Mostly Cloudy. Rain within 12 hours.");
setParameterValue("WEATHER_FORECAST", 2);
break;
case 0x12:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Mostly Cloudy. Snow within 12 hours.");
setParameterValue("WEATHER_FORECAST", 2);
break;
case 0x13:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Mostly Cloudy. Rain or Snow within 12 hours.");
setParameterValue("WEATHER_FORECAST", 2);
break;
case 0x07:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Partly Cloudy. Rain within 12 hours.");
setParameterValue("WEATHER_FORECAST", 1);
break;
case 0x16:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Partly Cloudy. Snow within 12 hours.");
setParameterValue("WEATHER_FORECAST", 1);
break;
case 0x17:
DEBUG(INDI::Logger::DBG_SESSION, "Forecast: Partly Cloudy. Rain or Snow within 12 hours.");
setParameterValue("WEATHER_FORECAST", 1);
break;
}
// Inside Temperature
uint16_t temperatureValue = loopData[10] << 8 | loopData[9];
setParameterValue("WEATHER_TEMPERATURE", ( (temperatureValue/10.0) - 32) / 1.8);
DEBUGF(INDI::Logger::DBG_DEBUG, "Raw Temperature (%d) [%#4X %#4X]", temperatureValue, loopData[9], loopData[10]);
// Barometer
uint16_t barometerValue = loopData[8] << 8 | loopData[7];
setParameterValue("WEATHER_BAROMETER", (barometerValue/1000.0) * 33.8639);
DEBUGF(INDI::Logger::DBG_DEBUG, "Raw Barometer (%d) [%#4X %#4X]", barometerValue, loopData[7], loopData[8]);
// Wind Speed
uint8_t windValue = loopData[14];
DEBUGF(INDI::Logger::DBG_DEBUG, "Raw Wind Speed (%d) [%#X4]", windValue, loopData[14]);
setParameterValue("WEATHER_WIND_SPEED", windValue / 0.62137);
// Wind Direction
uint16_t windDir = loopData[17] << 8 | loopData[16];
DEBUGF(INDI::Logger::DBG_DEBUG, "Raw Wind Direction (%d) [%#4X,%#4X]", windDir, loopData[16], loopData[17]);
setParameterValue("WEATHER_WIND_DIRECTION", windDir);
// Rain Rate
uint16_t rainRate = loopData[42] << 8 | loopData[41];
DEBUGF(INDI::Logger::DBG_DEBUG, "Raw Rain Rate (%d) [%#4X,%#4X]", rainRate, loopData[41], loopData[42]);
setParameterValue("WEATHER_RAIN_RATE", rainRate / (100 * 0.039370));
// Solar Radiation
uint16_t solarRadiation = loopData[45] << 8 | loopData[44];
DEBUGF(INDI::Logger::DBG_DEBUG, "Raw Solar Radiation (%d) [%#4X,%#4X]", solarRadiation, loopData[44], loopData[45]);
if (solarRadiation == 32767)
solarRadiation = 0;
setParameterValue("WEATHER_SOLAR_RADIATION", solarRadiation);
return IPS_OK;
}
static int
recover_one_file (logDev * dev, long long fseq, long long *seq, int *partial)
{
smrLogAddr *addr;
char *bp;
char *ep;
logChecksum *master;
logChecksum *checksums;
unsigned short csum;
int idx;
int modified;
int finalized;
int total_offset;
assert (fseq >= 0);
assert (seq_round_down (fseq) == fseq);
assert (seq != NULL);
assert (partial != NULL);
addr = dev->get_mmap (dev, fseq, 0, 0);
if (addr == NULL)
{
ERRNO_POINT ();
return -1;
}
/* adjust pointrs */
bp = addr->addr;
ep = bp + SMR_LOG_FILE_DATA_SIZE;
master = (logChecksum *) ep;
checksums = master + 1;
/* skip if finalized */
finalized = (master->off == SMR_LOG_NUM_CHECKSUM);
if (finalized)
{
*partial = 0;
*seq = fseq + SMR_LOG_FILE_DATA_SIZE;
dev->munmap (dev, addr);
return 0;
}
/* recover the log file from the start */
idx = 0;
modified = 0;
total_offset = 0;
while (idx < SMR_LOG_NUM_CHECKSUM - 1)
{
char *pagep = bp + idx * SMR_LOG_PAGE_SIZE;
int offset = checksums[idx].off;
csum = (offset == 0) ? 0 : crc16 (pagep, offset, 0);
if (csum != checksums[idx].checksum)
{
modified++;
checksums[idx].off = 0;
checksums[idx].checksum = 0;
break;
}
total_offset += offset;
if (offset < SMR_LOG_PAGE_SIZE)
{
break;
}
idx++;
}
/* set return value */
*seq = fseq + total_offset;
*partial = (total_offset != SMR_LOG_FILE_DATA_SIZE);
/* update master record */
if (master->off != idx)
{
modified++;
master->off = idx;
}
csum = crc16 ((char *) checksums, sizeof (logChecksum) * idx, 0);
if (csum != master->checksum)
{
modified++;
master->checksum = csum;
}
/* reset remaining checksum fields */
idx++;
while (idx < SMR_LOG_NUM_CHECKSUM - 1)
{
if (checksums[idx].off != 0)
{
modified++;
checksums[idx].off = 0;
}
if (checksums[idx].checksum != 0)
{
modified++;
checksums[idx].checksum = 0;
}
idx++;
}
if (modified && addr->loc == IN_DISK)
{
msync (addr->addr, SMR_LOG_FILE_ACTUAL_SIZE, MS_ASYNC);
}
dev->munmap (dev, addr);
return 0;
}
int main(void)
{
// Make sure all interrupts are disabled.
// This must be the first step, because in some cases interupt vectors are totally wrong
// (e.g. when we get here after a soft reboot from another application)
msp430ClearAllInterruptsNosave();
msp430WatchdogStop();
ledsInit();
flashLeds(LEDS_BOOTLOADER_START);
BootParams_t bootParams;
intFlashRead(BOOT_PARAMS_ADDRESS, &bootParams, sizeof(bootParams));
++bootParams.bootRetryCount;
if (bootParams.bootRetryCount > MAX_RETRY_COUNT) {
bootParams.bootRetryCount = 0;
bootParams.extFlashAddress = GOLDEN_IMAGE_ADDRESS;
bootParams.doReprogramming = 1;
}
// make sure internal flash address is sane
if (bootParams.intFlashAddress == 0xffff || bootParams.intFlashAddress < BOOTLOADER_END) {
bootParams.intFlashAddress = SYSTEM_CODE_START;
}
// read voltage, and quit if not enough for writing in flash
if (readVoltage() < THRESHOLD_VOLTAGE) {
flashLeds(LEDS_LOW_BATTERY);
goto exec;
}
// write the updated info back in flash
intFlashErase(BOOT_PARAMS_ADDRESS, sizeof(bootParams));
intFlashWrite(BOOT_PARAMS_ADDRESS, &bootParams, sizeof(bootParams));
if (bootParams.doReprogramming) {
redLedOn();
// will be using external flash
extFlashInit();
extFlashWake();
uint32_t extAddress = bootParams.extFlashAddress;
// read number of blocks
uint16_t imageBlockCount;
extFlashRead(extAddress, &imageBlockCount, sizeof(uint16_t));
extAddress += 2;
while (imageBlockCount) {
// read a block from external flash
ExternalFlashBlock_t block;
extFlashRead(extAddress, &block, sizeof(block));
if (block.crc != crc16((uint8_t *)&block, sizeof(block) - 2)) {
// the best we can do is to reboot now;
// after a few tries the golden image will be loaded
flashLeds(LEDS_CRC_ERROR);
// no need to disable all of the interrupts (they already are),
// just write in watchdog timer wihout password, it will generate reset.
watchdogRebootSimple();
}
bool firstBlockInChunk = block.address & 0x1;
block.address &= ~0x1;
if (firstBlockInChunk) {
// prepare internal flash to be written
intFlashErase(block.address, INT_FLASH_SEGMENT_SIZE);
}
// program internal flash
COMPILE_TIME_ASSERT(sizeof(block.data) == INT_FLASH_BLOCK_SIZE, ifs);
intFlashWriteBlock(block.address, block.data, INT_FLASH_BLOCK_SIZE);
--imageBlockCount;
extAddress += sizeof(ExternalFlashBlock_t);
}
extFlashSleep();
redLedOff();
}
#if ENABLE_BOOT_DELAY
// delay for a second or so to allow the user to interrupt booting (by pressing the reset button)
flashLeds(LEDS_BOOTLOADER_END);
#endif
// execute the program
exec:
((ApplicationStartExec)bootParams.intFlashAddress)();
}
bool SDHAL_WriteData(alt_u8 szDataWrite[], int nDataLen){
bool bSuccess = TRUE;
// int nTry = 0;
// const int nMaxTry = 5000;
int i, j;
alt_u8 Data8;
alt_u16 DataCrc16;
DataCrc16 = crc16(szDataWrite, nDataLen);
/*
// wait ready
while(1){
SD_CLK_LOW;
SD_CLK_HIGH;
if((SD_TEST_DAT & 0x01) == 0x00) // check bit0
break;
if (nTry++ > nMaxTry)
return FALSE;
} */
SD_DAT_OUT;
// start bits (zero value)
SD_CLK_LOW;
SD_DAT_WRITE(0x00);
SD_CLK_HIGH;
// write data (512byte = 1 block)
for(i=0;i<nDataLen;i++)
{
Data8 = szDataWrite[i];
#ifdef SD_4BIT_MODE
for(j=0;j<2;j++)
{
SD_CLK_LOW;
//
SD_DAT_WRITE((Data8 >> 4) & 0x0F);
//
SD_CLK_HIGH;
Data8 <<= 4;
}
#else
for(j=0;j<8;j++)
{
SD_CLK_LOW;
//
if (Data8 & 0x80)
SD_DAT_HIGH;
else
SD_DAT_LOW;
//
SD_CLK_HIGH;
Data8 <<= 1;
}
#endif
}
#ifdef SD_4BIT_MODE
// not implement yet
#else
// send CRC
for(i=0;i<16;i++){
SD_CLK_LOW;
if (DataCrc16 & 0x8000)
SD_DAT_HIGH;
else
SD_DAT_LOW;
//
SD_CLK_HIGH;
DataCrc16 <<= 1;
}
#endif
// stop bits (value 'one')
SD_CLK_LOW;
#ifdef SD_4BIT_MODE
SD_DAT_WRITE(0x0F);
#else
SD_DAT_HIGH;
#endif
SD_CLK_HIGH;
//===== check busy bits (data0 only)
SD_DAT_IN;
bool bWriteSuccess = FALSE;
for(i=0;i<32 && !bWriteSuccess;i++){
SD_CLK_LOW;
SD_CLK_HIGH;
if ((SD_TEST_DAT & 0x01) == 0x01) // (DAT0==LOW: busy indicate
bWriteSuccess = TRUE;
}
if (!bWriteSuccess)
bSuccess = FALSE;
// to provide8 (eight) clock cycles for the card to complete the operation before shutting down the clock
SDHAL_DummyClock(8);
/*
//
for(i=0; i<16; i++){
SD_CLK_LOW;
SD_CLK_HIGH;
}*/
return bSuccess;
}
static int decode_pkg(struct thr_info *thr, struct avalon2_ret *ar, uint8_t *pkg)
{
struct cgpu_info *avalon2;
struct avalon2_info *info;
struct pool *pool;
unsigned int expected_crc;
unsigned int actual_crc;
uint32_t nonce, nonce2, miner, modular_id;
int pool_no;
uint8_t job_id[5];
int tmp;
int type = AVA2_GETS_ERROR;
if (thr) {
avalon2 = thr->cgpu;
info = avalon2->device_data;
}
memcpy((uint8_t *)ar, pkg, AVA2_READ_SIZE);
if (ar->head[0] == AVA2_H1 && ar->head[1] == AVA2_H2) {
expected_crc = crc16(ar->data, AVA2_P_DATA_LEN);
actual_crc = (ar->crc[0] & 0xff) |
((ar->crc[1] & 0xff) << 8);
type = ar->type;
applog(LOG_DEBUG, "Avalon2: %d: expected crc(%04x), actural_crc(%04x)", type, expected_crc, actual_crc);
if (expected_crc != actual_crc)
goto out;
memcpy(&modular_id, ar->data + 28, 4);
modular_id = be32toh(modular_id);
if (modular_id == 3)
modular_id = 0;
switch(type) {
case AVA2_P_NONCE:
memcpy(&miner, ar->data + 0, 4);
memcpy(&pool_no, ar->data + 4, 4);
memcpy(&nonce2, ar->data + 8, 4);
/* Calc time ar->data + 12 */
memcpy(&nonce, ar->data + 16, 4);
memset(job_id, 0, 5);
memcpy(job_id, ar->data + 20, 4);
miner = be32toh(miner);
pool_no = be32toh(pool_no);
if (miner >= AVA2_DEFAULT_MINERS ||
modular_id >= AVA2_DEFAULT_MINERS ||
pool_no >= total_pools ||
pool_no < 0) {
applog(LOG_DEBUG, "Avalon2: Wrong miner/pool/id no %d,%d,%d", miner, pool_no, modular_id);
break;
} else
info->matching_work[modular_id * AVA2_DEFAULT_MINERS + miner]++;
nonce2 = bswap_32(nonce2);
nonce = be32toh(nonce);
nonce -= 0x180;
applog(LOG_DEBUG, "Avalon2: Found! [%s] %d:(%08x) (%08x)",
job_id, pool_no, nonce2, nonce);
/* FIXME:
* We need remember the pre_pool. then submit the stale work */
pool = pools[pool_no];
if (job_idcmp(job_id, pool->swork.job_id))
break;
if (thr && !info->new_stratum)
submit_nonce2_nonce(thr, pool_no, nonce2, nonce);
break;
case AVA2_P_STATUS:
memcpy(&tmp, ar->data, 4);
tmp = be32toh(tmp);
info->temp[0 + modular_id * 2] = tmp >> 16;
info->temp[1 + modular_id * 2] = tmp & 0xffff;
memcpy(&tmp, ar->data + 4, 4);
tmp = be32toh(tmp);
info->fan[0 + modular_id * 2] = tmp >> 16;
info->fan[1 + modular_id * 2] = tmp & 0xffff;
memcpy(&(info->get_frequency[modular_id]), ar->data + 8, 4);
memcpy(&(info->get_voltage[modular_id]), ar->data + 12, 4);
memcpy(&(info->local_work[modular_id]), ar->data + 16, 4);
memcpy(&(info->hw_work[modular_id]), ar->data + 20, 4);
info->get_frequency[modular_id] = be32toh(info->get_frequency[modular_id]);
info->get_voltage[modular_id] = be32toh(info->get_voltage[modular_id]);
info->local_work[modular_id] = be32toh(info->local_work[modular_id]);
info->hw_work[modular_id] = be32toh(info->hw_work[modular_id]);
info->local_works[modular_id] += info->local_work[modular_id];
info->hw_works[modular_id] += info->hw_work[modular_id];
info->get_voltage[modular_id] = decode_voltage(info->get_voltage[modular_id]);
avalon2->temp = get_temp_max(info);
break;
case AVA2_P_ACKDETECT:
break;
case AVA2_P_ACK:
break;
case AVA2_P_NAK:
break;
default:
type = AVA2_GETS_ERROR;
break;
}
}
// This function will first return non-reliable data,
// and then one or many reliable packets - it will modify bs for that,
// so you should call it in a loop, clearing bs after each call.
bool CChannel3::Process(CBytestream *bs)
{
bs->ResetPosToBegin();
if( bs->GetLength() == 0 )
return GetPacketFromBuffer(bs);
UpdateReceiveStatistics( bs->GetLength() );
// CRC16 check
unsigned crc = bs->readInt(2);
if( crc != crc16( bs->peekData( bs->GetRestLen() ).c_str(), bs->GetRestLen() ) )
{
iPacketsDropped++; // Update statistics
return GetPacketFromBuffer(bs); // Packet from the past or from too distant future - ignore it.
}
// Acknowledged packets info processing
// Read acknowledged packets indexes
unsigned seqAck = bs->readInt(2);
std::vector< int > seqAckList;
while( seqAck & SEQUENCE_HIGHEST_BIT )
{
seqAckList.push_back( seqAck & ~ SEQUENCE_HIGHEST_BIT );
seqAck = bs->readInt(2);
}
if( SequenceDiff( seqAck, LastReliableOut ) < 0 || SequenceDiff( seqAck, LastReliableOut ) > SEQUENCE_SAFE_DIST )
{
iPacketsDropped++; // Update statistics
return GetPacketFromBuffer(bs); // Packet from the past or from too distant future - ignore it.
}
LastReliableOut = seqAck;
iPacketsGood++; // Update statistics
// Delete acknowledged packets from buffer
for( PacketList_t::iterator it = ReliableOut.begin(); it != ReliableOut.end(); )
{
bool erase = false;
if( SequenceDiff( LastReliableOut, it->idx ) >= 0 )
erase = true;
for( unsigned f=0; f<seqAckList.size(); f++ )
if( seqAckList[f] == it->idx )
erase = true;
if(erase)
it = ReliableOut.erase(it);
else
it++;
}
// Calculate ping ( with LastReliableOut, not with last packet - should be fair enough )
if( PongSequence != -1 && SequenceDiff( LastReliableOut, PongSequence ) >= 0 )
{
iPing = (int) ((tLX->currentTime - fLastPingSent).milliseconds());
PongSequence = -1;
// Traffic shaping occurs here - change DataPacketTimeout according to received ping
// Change the value slowly, to avoid peaks
DataPacketTimeout = ( iPing/1000.0f/DATA_PACKET_TIMEOUT_PING_COEFF + DataPacketTimeout*9.0f ) / 10.0f;
};
// Processing of arrived data packets
// Read packets info
std::vector< int > seqList;
std::vector< int > seqSizeList;
unsigned seq = bs->readInt(2);
while( seq & SEQUENCE_HIGHEST_BIT )
{
seqList.push_back( seq & ~ SEQUENCE_HIGHEST_BIT );
seqSizeList.push_back( bs->readInt(2) );
seq = bs->readInt(2);
}
seqList.push_back( seq );
seqSizeList.push_back( bs->readInt(2) );
// Put packets in buffer
for( unsigned f=0; f<seqList.size(); f++ )
{
if( seqSizeList[f] == 0 ) // Last reliable packet may have size 0, if we're received non-reliable-only net packet
continue; // Just skip it, it's fake packet
bool addPacket = true;
for( PacketList_t::iterator it = ReliableIn.begin(); it != ReliableIn.end() && addPacket; it++ )
if( it->idx == seqList[f] )
addPacket = false;
if( addPacket && SequenceDiff( seqList[f], LastReliableIn ) > 0 ) // Do not add packets from the past
{ // Packet not in buffer yet - add it
CBytestream bs1;
bs1.writeData( bs->readData( seqSizeList[f] & ~ SEQUENCE_HIGHEST_BIT ) );
ReliableIn.push_back( Packet_t( bs1, seqList[f], (seqSizeList[f] & SEQUENCE_HIGHEST_BIT) != 0 ) );
}
else // Packet is in buffer already
{
// We may check here if arrived packet is the same as packet in buffer, and print errors.
bs->Skip( seqSizeList[f] & ~ SEQUENCE_HIGHEST_BIT );
};
}
// Increase LastReliableIn until the first packet that is missing from sequence
while( true ) // I just love such constructs :P don't worry, I've put "break" inside the loop.
{
bool nextPacketFound = false;
int LastReliableInInc = LastReliableIn + 1; // Next value of LastReliableIn
if( LastReliableInInc >= SEQUENCE_WRAPAROUND )
LastReliableInInc = 0;
for( PacketList_t::iterator it = ReliableIn.begin(); it != ReliableIn.end() && !nextPacketFound; it++ )
if( it->idx == LastReliableInInc )
nextPacketFound = true;
if( nextPacketFound )
LastReliableIn = LastReliableInInc;
else
break;
}
// The ReliableIn list updated - sort it
ReliableIn.sort();
if( bs->GetRestLen() > 0 ) // Non-reliable data left in this packet
return true; // Do not modify bs, allow user to read non-reliable data at the end of bs
if( GetPacketFromBuffer(bs) ) // We can return some reliable packet
return true;
// We've got valid empty packet, or packet from future, return empty packet - bs->GetRestLen() == 0 here.
// It is required to update server statistics, so clients that don't send packets won't timeout.
return true;
}
bool SDHAL_ReadData(alt_u8 szBuf[], int nBufLen){
bool bSuccess = TRUE;
int nTry = 0;
const int nMaxTry = 5000;
int i, j,k,n=0;
alt_u8 DataTemp;
alt_u8 Data8;
#ifndef SD_4BIT_MODE
alt_u16 DataCrc16, MyCrc16;
#else
alt_u8 szBuf_0[128],szBuf_1[128],szBuf_2[128],szBuf_3[128];
alt_u16 DataCrc16_0,DataCrc16_1,DataCrc16_2,DataCrc16_3;
alt_u16 MyCrc16_0,MyCrc16_1,MyCrc16_2,MyCrc16_3;
alt_u8 Data8_0,Data8_1,Data8_2,Data8_3;
#endif
SD_DAT_IN;
// wait start bits (zero)
while(1){
SD_CLK_LOW;
SD_CLK_HIGH;
#ifdef SD_4BIT_MODE
if((SD_TEST_DAT & 0x0F) == 0x00) // check start bits (zero is expected)
#else
if((SD_TEST_DAT & 0x01) == 0x00) // check start bits (zero is expected)
#endif
break;
if (nTry++ > nMaxTry)
return FALSE;
}
// read data (512byte = 1 block)
#ifdef SD_4BIT_MODE
for(i=0;i<nBufLen/4;i++)
{
k = 0;
Data8 = 0;
Data8_0 = 0;
Data8_1 = 0;
Data8_2 = 0;
Data8_3 = 0;
for(j=0;j<8;j++)
{
SD_CLK_LOW;
SD_CLK_HIGH;
Data8 <<= 4;
Data8_0 <<= 1;
Data8_1 <<= 1;
Data8_2 <<= 1;
Data8_3 <<= 1;
DataTemp = SD_TEST_DAT;
Data8 |= (DataTemp & 0x0F);
Data8_0 |= (DataTemp & 0x01);
Data8_1 |= ((DataTemp >> 1) & 0x01);
Data8_2 |= ((DataTemp >> 2) & 0x01);
Data8_3 |= ((DataTemp >> 3) & 0x01);
k++;
if(k == 2)
{
szBuf[n++] = Data8;
Data8 = 0;
k = 0;
}
}
szBuf_0[i] = Data8_0;
szBuf_1[i] = Data8_1;
szBuf_2[i] = Data8_2;
szBuf_3[i] = Data8_3;
}
#else
for(i=0;i<nBufLen;i++) //512byte
{
Data8 = 0;
for(j=0;j<8;j++)
{
SD_CLK_LOW;
SD_CLK_HIGH;
Data8 <<= 1;
if(SD_TEST_DAT & 0x01) // check bit0
Data8 |= 0x01;
}
szBuf[i]=Data8;
}
#endif
//===== CRC16 and end-bit check (each channel is seperated)
#ifdef SD_4BIT_MODE
// Not implement yet
DataCrc16_0 = 0;
DataCrc16_1 = 0;
DataCrc16_2 = 0;
DataCrc16_3 = 0;
for(i=0;i<16;i++)
{
SD_CLK_LOW;
SD_CLK_HIGH;
DataCrc16_0 <<= 1;
DataCrc16_1 <<= 1;
DataCrc16_2 <<= 1;
DataCrc16_3 <<= 1;
DataTemp = SD_TEST_DAT;
if (DataTemp & 0x01)
DataCrc16_0 |= 0x01;
if(DataTemp & 0x02)
DataCrc16_1 |= 0x01;
if(DataTemp & 0x04)
DataCrc16_2 |= 0x01;
if(DataTemp & 0x08)
DataCrc16_3 |= 0x01;
}
// check end bit (value 'one' is expected
SD_CLK_LOW;
SD_CLK_HIGH;
if ((SD_TEST_DAT & 0x0F) != 0x0F)
bSuccess = FALSE;
// to provide8 (eight) clock cycles for the card to complete the operation before shutting down the clock
SDHAL_DummyClock(8);
// check crc
if (bSuccess){
MyCrc16_0 = crc16(szBuf_0, nBufLen/4);
if (MyCrc16_0 != DataCrc16_0)
bSuccess = FALSE;
}
if (bSuccess){
MyCrc16_1 = crc16(szBuf_1, nBufLen/4);
if (MyCrc16_1 != DataCrc16_1)
bSuccess = FALSE;
}
if (bSuccess){
MyCrc16_2 = crc16(szBuf_2, nBufLen/4);
if (MyCrc16_2 != DataCrc16_2)
bSuccess = FALSE;
}
if (bSuccess){
MyCrc16_3 = crc16(szBuf_3, nBufLen/4);
if (MyCrc16_3 != DataCrc16_3)
bSuccess = FALSE;
}
#else
// read rcr
DataCrc16 = 0;
for(i=0;i<16;i++){
SD_CLK_LOW;
SD_CLK_HIGH;
DataCrc16 <<= 1;
if (SD_TEST_DAT & 0x01)
DataCrc16 |= 0x01;
}
// check end bit (value 'one' is expected
SD_CLK_LOW;
SD_CLK_HIGH;
if ((SD_TEST_DAT & 0x01) != 0x01)
bSuccess = FALSE;
// to provide8 (eight) clock cycles for the card to complete the operation before shutting down the clock
SDHAL_DummyClock(8);
// check crc
if (bSuccess){
MyCrc16 = crc16(szBuf, nBufLen);
if (MyCrc16 != DataCrc16)
bSuccess = FALSE;
}
#endif
return bSuccess;
}
/*
* called by mux to signal the end of an AL-PDU.
*
* check the crc on the data in incoming buffer (incoming_data). if it
* is good, transfers the data to the outgoing buffer (outgoing_data).
*/
void H223ALReceiver::SendClosingFlag()
{
byte inbuf[MAX_AL_BUFFER_SIZE];
byte inbyte;
byte computedCRC,receivedCRC;
unsigned short computedCRC16,receivedCRC16;
int i;
int len=blevel(incoming_data);
//Check length
if (len <= 0)
return;
if (type == AL1)
{
/* just transfer incoming buffer to outgoing buffer.. */
for(i=0;i<len;i++)
{
bread(incoming_data,(void *) &inbyte);
bwrite(outgoing_data,(void *) &inbyte);
}
} else if (type == AL2) {
/* check one byte crc, if good transfer to outgoing. if not, put back
into incoming */
computedCRC = 0;
for(i=0;i<len-1;i++)
{
bread(incoming_data,(char *) inbuf+i);
crc8(&computedCRC,inbuf[i]);
}
bread(incoming_data,(char *) inbuf+(len-1));
receivedCRC = inbuf[len-1];
if (computedCRC == receivedCRC)
{
/* put data without crc into outgoing buf */
Debug("AL receiving good CRC\n");
for(i=0;i<len-1;i++)
bwrite(outgoing_data,(char *) inbuf + i);
} else {
/* put all data back into incoming_data, bad crc */
Debug("AL receiving bad CRC\n");
for(i=0;i<len;i++)
bwrite(incoming_data,(char *) inbuf + i);
}
} else if (type == AL3) {
/* check two byte crc, if good transfer to outgoing. if not, put back into
incoming */
computedCRC16 = 0;
for(i=0;i<len-2;i++)
{
bread(incoming_data,(char *) inbuf+i);
crc16((short *)&computedCRC16,inbuf[i]);
}
bread(incoming_data,(char *) inbuf+(len-2));
bread(incoming_data,(char *) inbuf+(len-1));
receivedCRC16 = inbuf[len-2];
receivedCRC16 = (receivedCRC16 << 8) | (inbuf[len-1]);
if (computedCRC16 == receivedCRC16)
{
/* put data without crc into outgoing buf */
Debug("AL receiving good CRC\n");
for(i=0;i<len-2;i++)
bwrite(outgoing_data,(char *) inbuf + i);
} else {
/* bad crc, send it anyway. could modify to send with an error msg */
Debug("AL receiving bad CRC\n");
for(i=0;i<len-2;i++)
bwrite(outgoing_data,(char *) inbuf + i);
}
}
//Call the event
OnIncommingData((BYTE *)outgoing_data->data,blevel(outgoing_data));
}
//------------------------------------------------------------------------------
static int32_t Receive_Packet (uint8_t *data, int32_t *length, uint32_t timeout)
{
uint16_t i, packet_size;
uint8_t c;
//uint16_t tempCRC;
*length = 0;
luaWdgReload();
if (Receive_Byte(&c, timeout) != 0)
{
luaWdgReload();
return -1;
}
luaWdgReload();
switch (c)
{
case SOH:
packet_size = PACKET_SIZE;
break;
case STX:
packet_size = PACKET_1K_SIZE;
break;
case EOT:
return 0;
case CA:
if ((Receive_Byte(&c, timeout) == 0) && (c == CA))
{
*length = -1;
luaWdgReload();
return 0;
}
else
{
luaWdgReload();
return -1;
}
case ABORT1:
case ABORT2:
luaWdgReload();
return 1;
default:
luaWdgReload();
return -1;
}
*data = c;
luaWdgReload();
for (i = 1; i < (packet_size + PACKET_OVERHEAD); i ++)
{
if (Receive_Byte(data + i, 10) != 0)
{
luaWdgReload();
return -1;
}
}
luaWdgReload();
if (data[PACKET_SEQNO_INDEX] != ((data[PACKET_SEQNO_COMP_INDEX] ^ 0xff) & 0xff))
{
return -1;
}
if (crc16(&data[PACKET_HEADER], packet_size + PACKET_TRAILER) != 0) {
return -1;
}
*length = packet_size;
return 0;
}
/*---------------------------------------------------------------------------*/
int
cgroups_radio_read(void *buf, unsigned short bufsize)
{
//printk("cgroups_radio_read()\n");
//u8_t footer[2];
int len;
int validCRC = 0;
pthread_mutex_lock(&read_lock);
if(packet_seen == 0)
{
pthread_mutex_unlock(&read_lock);
return 0;
}
/*Make a copy of the recieved message and release the lock for another packet*/
cgroups_message_t tempCopy = received_message;
PRINTF("CGROUPS RADIO: packet_seen freed\n");
packet_seen = 0;
PRINTF(" recieve data: %p of size : %u\n", received_message.payload,received_message.length);
len = tempCopy.length;
if(len > CGROUPS_RADIO_MAX_PACKET_LEN) {
/* Empty the packet buffer */
PRINTF("THE PACKET SIZE WAS TOO BIG\n");
int a = 0;
for(a ; a < tempCopy.length ; a++)
tempCopy.payload[a] = ' ';
packet_seen = 0;
//RIMESTATS_ADD(badsynch);
pthread_mutex_unlock(&read_lock);
return 0;
}
if(len > 0)
{
/* Read payload and two bytes of footer */
//PRINTF("cgroups_radio_read: len %d\n", len);
if(len < 2) //too short
{
PRINTF("THE LEN WAS TOO SMALL\n");
//RIMESTATS_ADD(tooshort);
}
else if(len - 2 > bufsize) //too long
{
PRINTF("BAD : Packet Length %hd, bufsize %hd\n ", tempCopy.length, bufsize );
len = 2;
//RIMESTATS_ADD(toolong);
}
else //right size
{
/*-----SIMULATE CC2420 CALCULATIONS-------*/
/*calc the crc */
uint16_t crc = crc16(tempCopy.payload, len-2);
// CC2420 sets highest order bit as crc valid flag, rest of that byte is LQI (set to max quality) - see p. 37 of datasheet
/*If valid set the valid bit*/
if(crc == *((uint16_t*)(tempCopy.payload+(len-2))))
{
/*Set the valid bit & LQI to max*/
tempCopy.payload[len - 1] = FOOTER1_CRC_OK | FOOTER1_CORRELATION;
// tempCopy.payload[len - 1] = 0xFF;
validCRC = 1;
// set second last byte to RSSI MAX(not implemented) - see p. 37 of datasheet
tempCopy.payload[len - 2] = 0;
}
/*-----------------------------------------*/
//PRINTF("THE right size\n");
PRINTF("GOOD : Packet Length %hd, bufsize %hd\n ", tempCopy.length, bufsize );
PRINTF("RECIEVEING ");
int j = 0;
for(j ; j < tempCopy.length ; j++)
PRINTF("%d", tempCopy.payload[j]);
PRINTF("\n");
if(tempCopy.payload[len - 1] & FOOTER1_CRC_OK)
{
/*Now pass the packet on up to the higher levels*/
memcpy(buf, tempCopy.payload, bufsize);
tempCopy.length = 0;
/*Collect info about the medium*/
cgroups_radio_last_rssi = tempCopy.payload[ len - 2];
cgroups_radio_last_correlation = tempCopy.payload[len - 1] & FOOTER1_CORRELATION;
PRINTF("The footer check passed\n");
// RIMESTATS_ADD(llrx);
}
else
{
PRINTF("Cgroups Radio:BAD CRC\n");
// RIMESTATS_ADD(badcrc);
len = 2;
}
}
}
// free up the locks for the next packet
PRINTF("CGROUPS RADIO: Free packet_seen\n");
packet_seen = 0;
up(&read_lock);
if(len < 2) {
return 0;
}
return len - 2; /* Remove two bytes for the footer*/
}
void CChannel3::Transmit(CBytestream *unreliableData)
{
UpdateReliableStreamBandwidthCounter();
#ifdef DEBUG
// Very simple laggy connection emulation - send next packet once per DEBUG_SIMULATE_LAGGY_CONNECTION_SEND_DELAY
if( DEBUG_SIMULATE_LAGGY_CONNECTION_SEND_DELAY > 0.0f )
{
if( DebugSimulateLaggyConnectionSendDelay > tLX->currentTime )
return;
DebugSimulateLaggyConnectionSendDelay = tLX->currentTime + DEBUG_SIMULATE_LAGGY_CONNECTION_SEND_DELAY;
}
#endif
CBytestream bs;
// Add acknowledged packets indexes
for( PacketList_t::iterator it = ReliableIn.begin(); it != ReliableIn.end(); it++ )
if( SequenceDiff( it->idx, LastReliableIn ) > 0 ) // Packets out of sequence
bs.writeInt( it->idx | SEQUENCE_HIGHEST_BIT, 2 );
bs.writeInt( LastReliableIn, 2 );
// Add reliable packet to ReliableOut buffer
while( (int)ReliableOut.size() < MaxNonAcknowledgedPackets && !Messages.empty() && ! ReliableStreamBandwidthLimitHit() )
{
LastAddedToOut ++ ;
if( LastAddedToOut >= SEQUENCE_WRAPAROUND )
LastAddedToOut = 0;
if( Messages.front().GetLength() > MAX_FRAGMENTED_PACKET_SIZE )
{
// Fragment the packet
Messages.front().ResetPosToBegin();
CBytestream bs;
bs.writeData( Messages.front().readData( MAX_FRAGMENTED_PACKET_SIZE ) );
ReliableOut.push_back( Packet_t( bs, LastAddedToOut, true ) );
bs.Clear();
bs.writeData( Messages.front().readData() );
Messages.front() = bs;
}
else
{
ReliableOut.push_back( Packet_t( Messages.front(), LastAddedToOut, false ) );
Messages.pop_front();
while( ! Messages.empty() &&
ReliableOut.back().data.GetLength() + Messages.front().GetLength() <= MAX_FRAGMENTED_PACKET_SIZE )
{
ReliableOut.back().data.Append( & Messages.front() );
Messages.pop_front();
}
}
}
// Check if other side acknowledged packets with indexes bigger than NextReliablePacketToSend,
// and roll NextReliablePacketToSend back to LastReliableOut.
if( ! ReliableOut.empty() )
{
for( PacketList_t::iterator it = ReliableOut.begin(), it1 = it++; it != ReliableOut.end(); it1 = it++ )
{
if( SequenceDiff( it->idx, it1->idx ) != 1 )
NextReliablePacketToSend = LastReliableOut;
}
if( ReliableOut.back().idx != LastAddedToOut )
NextReliablePacketToSend = LastReliableOut;
}
// Timeout occured - other side didn't acknowledge our packets in time - re-send all of them from the first one.
if( LastReliablePacketSent == LastAddedToOut &&
SequenceDiff( LastReliablePacketSent, LastReliableOut ) >= MaxNonAcknowledgedPackets &&
tLX->currentTime - fLastSent >= DataPacketTimeout )
{
NextReliablePacketToSend = LastReliableOut;
}
// Add packet headers and data - send all packets with indexes from NextReliablePacketToSend and up.
// Add older packets to the output first.
// NextReliablePacketToSend points to the last packet.
CBytestream packetData;
bool unreliableOnly = true;
bool firstPacket = true; // Always send first packet, even if it bigger than MAX_PACKET_SIZE
// This should not occur when packets are fragmented
int packetIndex = LastReliableOut;
int packetSize = 0;
for( PacketList_t::iterator it = ReliableOut.begin(); it != ReliableOut.end(); it++ )
{
if( SequenceDiff( it->idx, NextReliablePacketToSend ) >= 0 )
{
if( ! CheckReliableStreamBandwidthLimit( (float)(it->data.GetLength() + 4) ) ||
( bs.GetLength() + 4 + packetData.GetLength() + it->data.GetLength() > MAX_PACKET_SIZE-2 && !firstPacket ) ) // Substract CRC16 size
break;
if( !firstPacket )
{
bs.writeInt( packetIndex | SEQUENCE_HIGHEST_BIT, 2 );
bs.writeInt( packetSize, 2 );
};
packetIndex = it->idx;
packetSize = it->data.GetLength();
if( it->fragmented )
packetSize |= SEQUENCE_HIGHEST_BIT;
firstPacket = false;
unreliableOnly = false;
NextReliablePacketToSend = it->idx;
packetData.Append( &it->data );
}
}
bs.writeInt( packetIndex, 2 );
bs.writeInt( packetSize, 2 );
bs.Append( &packetData );
if( unreliableOnly )
bs.Append(unreliableData);
else
{
if( bs.GetLength() + unreliableData->GetLength() <= MAX_PACKET_SIZE-2 ) // Substract CRC16 size
bs.Append(unreliableData);
// If we are sending a reliable message, remember this time and use it for ping calculations
if (PongSequence == -1)
{
PongSequence = NextReliablePacketToSend;
fLastPingSent = tLX->currentTime;
}
}
if( unreliableData->GetLength() == 0 &&
LastReliablePacketSent == LastAddedToOut &&
tLX->currentTime - fLastSent < DataPacketTimeout )
{
// No unreliable data to send, and we've just sent the same packet -
// send it again after some timeout, don't flood net.
return;
}
if( unreliableData->GetLength() == 0 && packetData.GetLength() == 0 &&
LastReliableIn == LastReliableIn_SentWithLastPacket &&
tLX->currentTime - fLastSent < KeepAlivePacketTimeout )
{
// Nothing to send really, send one empty packet per halfsecond so we won't timeout,
// but always send first packet with acknowledges, or other side will flood
// non-acknowledged packets for halfsecond.
// CChannel_056b will always send packet on each frame, so we're conserving bandwidth compared to it, hehe.
cOutgoingRate.addData( tLX->currentTime, 0 );
return;
}
// Add CRC16
CBytestream bs1;
bs1.writeInt( crc16( bs.data().c_str(), bs.GetLength() ), 2);
bs1.Append(&bs);
// Send the packet
Socket->setRemoteAddress(RemoteAddr);
bs1.Send(Socket.get());
LastReliableIn_SentWithLastPacket = LastReliableIn;
LastReliablePacketSent = NextReliablePacketToSend;
UpdateTransmitStatistics( bs1.GetLength() );
}
bool PacketInterface::sendPacket(const unsigned char *data, unsigned int len_packet)
{
// Only allow firmware commands in limited mode
if (mIsLimitedMode && data[0] > COMM_WRITE_NEW_APP_DATA) {
return false;
}
static unsigned char buffer[mMaxBufferLen];
unsigned int ind = 0;
// If the IP is valid, send the packet over UDP
if (QString::compare(mHostAddress.toString(), "0.0.0.0") != 0) {
if (mSendCan) {
buffer[ind++] = COMM_FORWARD_CAN;
buffer[ind++] = mCanId;
}
memcpy(buffer + ind, data, len_packet);
ind += len_packet;
mUdpSocket->writeDatagram(QByteArray::fromRawData((const char*)buffer, ind), mHostAddress, mUdpPort);
return true;
}
int len_tot = len_packet;
if (mSendCan) {
len_tot += 2;
}
unsigned int data_offs = 0;
if (len_tot <= 256) {
buffer[ind++] = 2;
buffer[ind++] = len_tot;
data_offs = 2;
} else {
buffer[ind++] = 3;
buffer[ind++] = len_tot >> 8;
buffer[ind++] = len_tot & 0xFF;
data_offs = 3;
}
if (mSendCan) {
buffer[ind++] = COMM_FORWARD_CAN;
buffer[ind++] = mCanId;
}
memcpy(buffer + ind, data, len_packet);
ind += len_packet;
unsigned short crc = crc16(buffer + data_offs, len_tot);
buffer[ind++] = crc >> 8;
buffer[ind++] = crc;
buffer[ind++] = 3;
QByteArray sendData = QByteArray::fromRawData((char*)buffer, ind);
emit dataToSend(sendData);
return true;
}
