// Combined clock jitter techniques to return approximately 8 bits (the entire result byte) of entropy efficiently on demand.
// Expensive in terms of CPU time and thus energy, though possibly more efficient than basic clockJitterXXX() routines.
// Internally this uses a CRC as a relatively fast and hopefully effective hash over intermediate values.
// Note the that rejection of repeat values will be less effective with two interleaved gathering mechanisms
// as the interaction while not necessarily adding genuine entropy, will make counts differ between runs.
// DHD20130519: measured as taking ~63ms to run, ie ~8ms per bit gathered.
// NOTE: in this file to have direct access to WDT.
uint_fast8_t clockJitterEntropyByte()
{
uint16_t hash = 0;
uint_fast8_t result = 0;
uint_fast8_t countR = 0, lastCountR = 0;
uint_fast8_t countW = 0, lastCountW = 0;
const uint8_t t0 = TCNT2; // Wait for sub-cycle timer to roll.
while(t0 == TCNT2) { ++hash; } // Possibly capture some entropy from recent program activity/timing.
uint8_t t1 = TCNT2;
_watchdogFired = 0;
wdt_enable(WDTO_15MS); // Start watchdog, with minimum timeout.
WDTCSR |= (1 << WDIE);
int_fast8_t bitsLeft = 8; // Decrement when a bit is harvested...
for( ; ; )
{
// Extract watchdog jitter vs CPU.
if(!_watchdogFired) { ++countW; }
else // Watchdog fired.
{
if(countW != lastCountW) // Got a different value from last; assume one bit of entropy.
{
hash = _crc_ccitt_update(hash, countW);
result = (result << 1) ^ ((uint_fast8_t)hash); // Nominally capturing (at least) lsb of hash.
if(--bitsLeft <= 0) { break; } // Got enough bits; stop now.
lastCountW = countW;
}
countW = 0;
_watchdogFired = 0;
wdt_enable(WDTO_15MS); // Restart watchdog, with minimum timeout.
WDTCSR |= (1 << WDIE);
}
// Extract RTC jitter vs CPU.
if(t1 == TCNT2) { --countR; }
else // Sub-cycle timer rolled.
{
if(countR != lastCountR) // Got a different value from last; assume one bit of entropy.
{
hash = _crc_ccitt_update(hash, countR);
result = (result << 1) ^ ((uint_fast8_t)hash); // Nominally capturing (at least) lsb of hash.
if(--bitsLeft <= 0) { break; } // Got enough bits; stop now.
lastCountR = countR;
}
countR = 0;
t1 = TCNT2; // Set to look for next roll.
}
}
wdt_disable(); // Ensure no spurious WDT wakeup pending.
return(result);
}
void eeprom_read_data(const EepromContext *ctx,
void *data_ptr, const size_t data_len,
const void * const eep_data_ptr,
const void * const eep_crc_ptr,
const void * const default_data_ptr) {
/* Save the state of the EEPROM Ready Interrupt and then disable it */
uint8_t prev_eerie = EECR & _BV(EERIE);
EECR &= ~_BV(EERIE);
/* If the current context is being written, we just copy the write buffer
* to the read buffer. Otherwise we read the data from the EEPROM. */
if (active == ctx) {
memcpy(data_ptr, ctx->ram_ptr, data_len);
}
else {
eeprom_read_block(data_ptr, eep_data_ptr, data_len);
uint16_t calculated_crc = CRC_INIT;
uint8_t *ptr = (uint8_t *)data_ptr;
for (int i=0; i<data_len; ++i) {
calculated_crc = _crc_ccitt_update(calculated_crc, *ptr++);
}
uint16_t crc = eeprom_read_word(eep_crc_ptr);
if (crc != calculated_crc) {
eeprom_set_default_data(data_ptr, default_data_ptr, data_len);
}
}
/* Restore the previsously saved state for the EEPROM Ready Interrupt */
EECR |= prev_eerie;
}
// Compute CRC over count bytes.
// This should only be ever called at user level, not interrupt level
uint16_t vw_crc(uint8_t *ptr, uint8_t count)
{
uint16_t crc = 0xffff;
while (count-- > 0)
crc = _crc_ccitt_update(crc, *ptr++);
return crc;
}
/**
* Calculate check sum for given buffer and number of bytes with CRC.
* Return true(1) if equals correct CCITT 16b check sum else
* false(0).
* @param[in] ptr buffer pointer.
* @param[in] count number of bytes in buffer.
* @return bool.
*/
static bool
is_valid_crc(uint8_t* ptr, uint8_t count)
{
uint16_t crc = 0xffff;
while (count-- > 0)
crc = _crc_ccitt_update(crc, *ptr++);
return (crc == 0xf0b8);
}
// Wait until transmitter is available and encode and queue the message
// into vw_tx_buf
// The message is raw bytes, with no packet structure imposed
// It is transmitted preceded a byte count and followed by 2 FCS bytes
unsigned char vw_send(unsigned char* buf, unsigned char len)
{
uint8_t i;
uint8_t index = 0;
uint16_t crc = 0xffff;
uint8_t *p = vw_tx_buf + VW_HEADER_LEN; // start of the message area
uint8_t count = len + 3; // Added byte count and FCS to get total number of bytes
if (len > VW_MAX_PAYLOAD)
return false;
// Wait for transmitter to become available
vw_wait_tx();
// Encode the message length
crc = _crc_ccitt_update(crc, count);
p[index++] = symbols[count >> 4];
p[index++] = symbols[count & 0xf];
// Encode the message into 6 bit symbols. Each byte is converted into
// 2 6-bit symbols, high nybble first, low nybble second
for (i = 0; i < len; i++)
{
crc = _crc_ccitt_update(crc, buf[i]);
p[index++] = symbols[buf[i] >> 4];
p[index++] = symbols[buf[i] & 0xf];
}
// Append the fcs, 16 bits before encoding (4 6-bit symbols after encoding)
// Caution: VW expects the _ones_complement_ of the CCITT CRC-16 as the FCS
// VW sends FCS as low byte then hi byte
crc = ~crc;
p[index++] = symbols[(crc >> 4) & 0xf];
p[index++] = symbols[crc & 0xf];
p[index++] = symbols[(crc >> 12) & 0xf];
p[index++] = symbols[(crc >> 8) & 0xf];
// Total number of 6-bit symbols to send
vw_tx_len = index + VW_HEADER_LEN;
// Start the low level interrupt handler sending symbols
vw_tx_start();
return true;
}
uint8_t trx_frame_read_data_crc(uint8_t *data, uint8_t datasz,
uint8_t *lqi, bool *crc_ok)
{
uint8_t length = 0;
uint8_t i, tmp;
uint16_t crc16 = 0;
/* Select transceiver */
SPI_SELN_LOW();
/* start frame read */
SPI_DATA_REG = TRX_CMD_FR;
SPI_WAITFOR();
/* read length */
SPI_DATA_REG = 0;
SPI_WAITFOR();
length = SPI_DATA_REG;
if ( (length - 2) <= datasz)
{
i = length;
do
{
SPI_DATA_REG = 0; /* dummy out */
SPI_WAITFOR();
tmp = SPI_DATA_REG;
crc16 = _crc_ccitt_update(crc16, tmp);
if (i > 1)
{
*data++ = tmp;
}
}
while(--i);
if(crc_ok != NULL)
{
*crc_ok = (crc16 == 0) ? true : false;
}
if (lqi != NULL)
{
SPI_DATA_REG = 0; /* dummy out */
SPI_WAITFOR();
*lqi = SPI_DATA_REG;
}
}
else
{
/* we drop the frame */
length = 0x80 | length;
}
SPI_SELN_HIGH();
return length;
}
uint16_t ubcrc16_data(uint8_t *data, uint8_t len)
{
uint16_t crc = 0xFFFF;
uint8_t i = 0;
//crc = _crc_ccitt_update(crc, len);
//tx('A');
for(i=0; i<len; i++){
//tx(data[i]);
crc = _crc_ccitt_update(crc, data[i]);
}
//tx('B');
return crc;
}
void eeprom_write_data(EepromContext *ctx,
void *eep_data_ptr, void *eep_crc_ptr,
const void * const data_ptr, size_t data_len) {
/* Disable the EEPROM Ready Interrupt */
EECR &= ~_BV(EERIE);
/* Set up the context */
ctx->eep_addr = (uint16_t)eep_data_ptr;
ctx->eep_crc_addr = (uint16_t)eep_crc_ptr;
ctx->ram_ptr = (const uint8_t *)data_ptr;
ctx->data_len = data_len;
if (active == NULL) {
/* If no writer is active, just go */
active = ctx;
ctx->next = NULL;
write_ram_ptr = ctx->ram_ptr;
write_state = WRITE_STATE_DATA;
}
else if (ctx == active) {
/* Restart if the active writer is the same context */
write_ram_ptr = ctx->ram_ptr;
write_state = WRITE_STATE_DATA;
}
else {
/* There are at least one other active job. Put our job in the queue */
EepromContext *prev = active;
EepromContext *it = active->next;
while (it != NULL) {
prev = it;
it = it->next;
}
prev->next = ctx;
ctx->next = NULL;
}
/* Caclulate CRC for the data block */
crc = CRC_INIT;
const uint8_t *ptr = (const uint8_t *)data_ptr;
for (int i=0; i<data_len; ++i) {
crc = _crc_ccitt_update(crc, *ptr++);
}
/* Enable the EEPROM Ready Interrupt */
EECR |= (1 << EERIE);
}
void ax25_frame(char *scallsign, char sssid, char *dcallsign, char dssid,
char *path1, char ttl1, char *path2, char ttl2, char *data, ...)
{
static uint8_t frame[100];
uint8_t *s;
uint16_t x;
va_list va;
va_start(va, data);
/* Pause while there is still data transmitting */
while(_txlen);
/* Write in the callsigns and paths */
s = _ax25_callsign(frame, dcallsign, dssid);
s = _ax25_callsign(s, scallsign, sssid);
if(path1) s = _ax25_callsign(s, path1, ttl1);
if(path2) s = _ax25_callsign(s, path2, ttl2);
/* Mark the end of the callsigns */
s[-1] |= 1;
*(s++) = 0x03; /* Control, 0x03 = APRS-UI frame */
*(s++) = 0xF0; /* Protocol ID: 0xF0 = no layer 3 data */
vsnprintf((char *) s, 100 - (s - frame) - 2, data, va);
va_end(va);
/* Calculate and append the checksum */
for(x = 0xFFFF, s = frame; *s; s++)
x = _crc_ccitt_update(x, *s);
*(s++) = ~(x & 0xFF);
*(s++) = ~((x >> 8) & 0xFF);
/* Point the interrupt at the data to be transmit */
_txbuf = frame;
_txlen = s - frame;
/* Enable the timer and key the radio */
TIMSK2 |= _BV(TOIE2);
PORTA |= TXENABLE;
}
// -----------------------------------------------------------------------------
void ProcessDataFromNav(void)
{
volatile uint8_t * rx_buffer_ptr =
(volatile uint8_t *)&from_nav_[from_nav_head_];
uint16_t crc = 0xFFFF;
for (uint8_t i = sizeof(struct FromNav) - 2; i--; )
crc = _crc_ccitt_update(crc, *(rx_buffer_ptr++));
if (from_nav_[from_nav_head_].crc == crc)
{
// Swap buffers.
from_nav_tail_ = from_nav_head_;
from_nav_head_ = !from_nav_tail_;
RedLEDOff();
}
nav_data_state_ = NAV_COMMS_IDLE;
}
/** @brief Compute the CRC of a given memory range.
*
* The client asks for a CRC, providing an address and a size, and the server
* replies with the computed CRC.
*
* Parameters:
* - start address (u32)
* - size (u32)
*
* Reply: computed CRC (u16)
*/
static void cmd_mem_crc(void)
{
const addr_type start = recv_addr();
const addr_type size = recv_addr();
#ifndef DISABLE_STRICT_CHECKS
if( start + size > FLASHEND+1 ) {
reply_error(STATUS_BAD_VALUE);
return;
}
#endif
// Compute CRC
uint16_t crc = 0xffff;
addr_type addr;
for( addr=start; addr<start+size; addr++ ) {
const uint8_t c = pgm_read_byte_bootloader(addr);
crc = _crc_ccitt_update(crc, c);
}
reply_success(2);
send_u16(crc);
}
uint8_t wibo_run(void)
{
uint8_t isLeave=0;
uint8_t isStay=0;
unsigned long timeout = WIBO_TIMEOUT;
while(!isLeave) {
#if !defined(NO_LEDS)
LED_CLR(PROGLED);
#endif
if (!(isStay))
{
while(!(wibo_available()) && (timeout--)) _delay_ms(1); // minimum frame time @ 250kbps ~ 2ms.
if (!(wibo_available())) // no packets received, bye bye!
{
isLeave=1;
return isLeave;
}
}
else
{
while(!(wibo_available())); // wait for next packet
}
trx_reg_write(RG_IRQ_STATUS, TRX_IRQ_RX_END); /* clear the flag */
trx_frame_read(rxbuf.data, sizeof(rxbuf.data) / sizeof(rxbuf.data[0]),
&tmp); /* dont use LQI, write into tmp variable */
#if !defined(NO_LEDS)
LED_SET(PROGLED);
/* light as long as actions are running */
#endif
switch (rxbuf.hdr.cmd)
{
case P2P_PING_REQ:
isStay=1;
if (0 == deaf)
{
pingrep.hdr.dst = rxbuf.hdr.src;
pingrep.hdr.seq++;
pingrep.crc = datacrc;
trx_reg_write(RG_TRX_STATE, CMD_TX_ARET_ON);
/* no need to make block atomic since no IRQs are used */
TRX_SLPTR_HIGH()
;
TRX_SLPTR_LOW()
;
trx_frame_write(sizeof(p2p_ping_cnf_t) + sizeof(BOARD_NAME) + 2,
(uint8_t*) &pingrep);
/*******************************************************/
#if defined(_DEBUG_SERIAL_)
printf("Pinged by 0x%04X"EOL, rxbuf.hdr.src);
#endif
#if defined(TRX_IF_RFA1)
while (!(trx_reg_read(RG_IRQ_STATUS) & TRX_IRQ_TX_END))
;
trx_reg_write(RG_IRQ_STATUS, TRX_IRQ_TX_END); /* clear the flag */
#else
while (!(trx_reg_read(RG_IRQ_STATUS) & TRX_IRQ_TRX_END))
;
#endif /* defined(TRX_IF_RFA1) */
trx_reg_write(RG_TRX_STATE, CMD_RX_AACK_ON);
} /* (0 == deaf) */
break;
case P2P_WIBO_TARGET:
isStay=1;
target = rxbuf.wibo_target.targmem;
#if defined(_DEBUG_SERIAL_)
printf("Set Target to %c"EOL, target);
#endif
break;
case P2P_WIBO_RESET:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Reset"EOL);
#endif
addr = SPM_PAGESIZE; /* misuse as counter */
ptr = pagebuf;
do
{
*ptr++ = 0xFF;
} while (--addr);
addr = 0;
datacrc = 0;
pagebufidx = 0;
deaf = 0;
break;
case P2P_WIBO_ADDR:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Set address: 0x%08lX"EOL, rxbuf.wibo_addr.address);
#endif
addr = rxbuf.wibo_addr.address;
pagebufidx = 0;
break;
case P2P_WIBO_DATA:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Data[%d]", rxbuf.wibo_data.dsize);
uint8_t len = rxbuf.wibo_data.dsize;
if (len > 10) len = 10;
for(uint8_t j=0;j<len;j++)
{
printf(" %02X", rxbuf.wibo_data.data[j]);
}
if (len != rxbuf.wibo_data.dsize)
printf("...");
printf(EOL);
#endif
tmp = rxbuf.wibo_data.dsize;
ptr = rxbuf.wibo_data.data;
do
{
datacrc = _crc_ccitt_update(datacrc, *ptr);
pagebuf[pagebufidx++] = *ptr;
if (pagebufidx >= PAGEBUFSIZE)
{
/* LED off to save current and avoid flash corruption
* because of possible voltage drops
*/
#if !defined(NO_LEDS)
LED_CLR(PROGLED);
#endif
if (target == 'F') /* Flash memory */
{
boot_program_page(addr, pagebuf);
}
else if (target == 'E')
{
/* not implemented */
}
else
{
/* unknown target, dry run */
}
/* also for dry run! */
addr += SPM_PAGESIZE;
pagebufidx = 0;
}
ptr++;
} while (--tmp);
break;
#if defined(WIBO_FLAVOUR_BOOTLUP)
case P2P_WIBO_BOOTLUP:
isStay=1;
bootlup();
break;
#endif
case P2P_WIBO_FINISH:
isStay=1;
#if defined(_DEBUG_SERIAL_)
printf("Finish"EOL);
#endif
if (target == 'F') /* Flash memory */
{
boot_program_page(addr, pagebuf);
}
else if (target == 'E')
{
/* not implemented */
}
else
{
/* unknown target, dry run */
}
/* also for dry run! */
addr += SPM_PAGESIZE;
pagebufidx = 0;
break;
case P2P_WIBO_EXIT:
#if defined(_DEBUG_SERIAL_)
printf("Exit"EOL);
#endif
#if !defined(NO_LEDS)
LED_CLR(PROGLED);
#endif
isLeave=1;
break;
case P2P_WIBO_DEAF:
isStay=1;
deaf = 1;
break;
default:
/* unknown or unhandled command */
if (!(isStay)) {
if (!(timeout--)) {
isLeave = 1;
}
}
break;
}; /* switch (rxbuf.hdr.cmd) */
}
return isLeave;
}
// -----------------------------------------------------------------------------
void ExchangeDataWithNav(void)
{
// Request the SPI transmit buffer. Return if not available.
uint8_t * tx_buffer = RequestSPITxBuffer();
if (tx_buffer == 0) return;
// Specify the payload structure.
struct ToNav {
uint16_t timestamp;
uint16_t state;
int16_t accelerometer[3];
int16_t gyro[3];
float quaternion[4];
#ifdef LOG_FLT_CTRL_DEBUG_TO_SD
int16_t sbus_pitch;
int16_t sbus_roll;
int16_t sbus_yaw;
int16_t sbus_thrust;
int16_t sbus_on_off;
uint16_t battery_voltage;
float heading;
float quaternion_command[4];
float heading_command;
float angular_command[3];
float attitude_integral[3];
float quaternion_model[4];
uint16_t motor_setpoints[8];
#endif
} __attribute__((packed));
_Static_assert(2 + sizeof(struct ToNav) + 2 + 4 < SPI_TX_BUFFER_LENGTH,
"struct ToNav is too large for the SPI TX buffer");
// Populate the transmit buffer.
tx_buffer[0] = NAV_MESSAGE_START_BYTE;
tx_buffer[1] = sizeof(struct ToNav);
struct ToNav * to_nav_ptr = (struct ToNav *)&tx_buffer[2];
to_nav_ptr->accelerometer[0] = AccelerometerSum(X_BODY_AXIS);
to_nav_ptr->accelerometer[1] = AccelerometerSum(Y_BODY_AXIS);
to_nav_ptr->accelerometer[2] = AccelerometerSum(Z_BODY_AXIS);
to_nav_ptr->gyro[0] = GyroSum(X_BODY_AXIS);
to_nav_ptr->gyro[1] = GyroSum(Y_BODY_AXIS);
to_nav_ptr->gyro[2] = GyroSum(Z_BODY_AXIS);
to_nav_ptr->quaternion[0] = Quat()[0];
to_nav_ptr->quaternion[1] = Quat()[1];
to_nav_ptr->quaternion[2] = Quat()[2];
to_nav_ptr->quaternion[3] = Quat()[3];
#ifdef LOG_FLT_CTRL_DEBUG_TO_SD
to_nav_ptr->sbus_pitch = SBusPitch();
to_nav_ptr->sbus_roll = SBusRoll();
to_nav_ptr->sbus_yaw = SBusYaw();
to_nav_ptr->sbus_thrust = SBusThrust();
to_nav_ptr->sbus_on_off = SBusOnOff();
to_nav_ptr->state = State();
to_nav_ptr->battery_voltage = BatteryVoltage();
to_nav_ptr->heading = HeadingAngle();
to_nav_ptr->quaternion_command[0] = QuatCommandVector()[0];
to_nav_ptr->quaternion_command[1] = QuatCommandVector()[1];
to_nav_ptr->quaternion_command[2] = QuatCommandVector()[2];
to_nav_ptr->quaternion_command[3] = QuatCommandVector()[3];
to_nav_ptr->heading_command = HeadingCommand();
to_nav_ptr->angular_command[0] = AngularCommand(0);
to_nav_ptr->angular_command[1] = AngularCommand(1);
to_nav_ptr->angular_command[2] = AngularCommand(2);
to_nav_ptr->attitude_integral[0] = AttitudeIntegralVector()[0];
to_nav_ptr->attitude_integral[1] = AttitudeIntegralVector()[1];
to_nav_ptr->attitude_integral[2] = AttitudeIntegralVector()[2];
to_nav_ptr->quaternion_model[0] = QuatModelVector()[0];
to_nav_ptr->quaternion_model[1] = QuatModelVector()[1];
to_nav_ptr->quaternion_model[2] = QuatModelVector()[2];
to_nav_ptr->quaternion_model[3] = QuatModelVector()[3];
to_nav_ptr->motor_setpoints[0] = MotorSetpoint(0);
to_nav_ptr->motor_setpoints[1] = MotorSetpoint(1);
to_nav_ptr->motor_setpoints[2] = MotorSetpoint(2);
to_nav_ptr->motor_setpoints[3] = MotorSetpoint(3);
to_nav_ptr->motor_setpoints[4] = MotorSetpoint(4);
to_nav_ptr->motor_setpoints[5] = MotorSetpoint(5);
to_nav_ptr->motor_setpoints[6] = MotorSetpoint(6);
to_nav_ptr->motor_setpoints[7] = MotorSetpoint(7);
to_nav_ptr->timestamp = GetTimestamp();
#endif
// Add a CRC just after the data payload. Note that this is the same CRC that
// is used in MAVLink. It uses a polynomial represented by 0x1021, an initial
// value of 0xFFFF, and with both the input and output reflected.
union U16Bytes * crc = (union U16Bytes *)&tx_buffer[2 + sizeof(struct ToNav)];
crc->u16 = 0xFFFF;
uint8_t * tx_buffer_ptr = &tx_buffer[1]; // Skip the start byte
for (uint8_t i = sizeof(struct ToNav) + 1; i--; )
crc->u16 = _crc_ccitt_update(crc->u16, *tx_buffer_ptr++);
// The MikroKopter NaviCtrl board employs a STR91x microcontroller. The SPI Rx
// interrupt for this board only occurs when the Rx buffer (8 bytes) is more
// than half full. To make sure that this interrupt is triggered, add 4
// trailing bytes to the end of the transmission.
tx_buffer_ptr += sizeof(uint16_t); // Skip the CRC
for (uint8_t i = 4; i--; ) *tx_buffer_ptr++ = 0x00;
// Send the entire packet (1-byte header, 1-byte length, payload, 2-byte CRC &
// 4-bytes trailing zeros).
SPIExchangeThenCallback(2 + sizeof(struct ToNav) + 2 + 4,
(volatile uint8_t *)&from_nav_[from_nav_head_], sizeof(struct FromNav),
SetNavDataReceived);
nav_data_state_ = NAV_COMMS_DATA_IN_PROGRESS;
}
bool hex_receiver::decode (uint16_t char_in)
{
// If we get a return character, expect that it will be followed by the
// beginning of some useful data and reset the decoder
if ((char_in == '\n') || (char_in == '\r'))
{
decoder_state = 0;
}
else
{
switch (decoder_state)
{
// State 0: We haven't received any characters yet; the first one is
// the first character of the type code. Reset everything to
// condition at start of reading process
case (0):
good_data = false;
computed_crc = 0xFFFF;
type_code = ascii_to_nibble (char_in) << 4;
p_data = NULL;
decoder_state = 1;
break;
// State 1: Get the second nibble of the command code
case (1):
type_code |= ascii_to_nibble (char_in);
decoder_state = 2;
HXR_DBG (p_serial, hex << PMS ("Code: ") << type_code << endl);
// OK, we have the code; use it to get a pointer to the object into
// which we'll be writing the data
p_map = p_first_map;
while (p_map != NULL)
{
// If debugging, show what we're looking at
HXR_DBG (p_serial, PMS ("HXR ptr: ") << p_map << endl);
// If we have a matching code, get the corresponding data pointer
if (p_map->code == type_code)
{
p_data = (uint8_t*)(p_map->p_obj);
HXR_DBG (p_serial, PMS (" dptr: ") << p_data << endl);
break;
}
else
{
HXR_DBG (p_serial, PMS ("HXR ") << hex << p_map->code
<< PMS (" != ") << type_code << endl);
}
// If no match found, go to the next item in the list
p_map = p_map->p_next;
}
break;
// State 2: Get the first nibble of the package size
case (2):
size = ascii_to_nibble (char_in) << 4;
decoder_state = 3;
break;
// State 3: Get the second nibble of the package size
case (3):
size |= ascii_to_nibble (char_in);
HXR_DBG (p_serial, PMS (" size: ") << size << hex << endl);
decoder_state = 4;
break;
// State 4: Get the first nibble of whatever is next
case (4):
num_in = ascii_to_nibble (char_in) << 4;
decoder_state = 5;
break;
// State 5: Get the second nibble of whatever is next. If this isn't
// the last item of data, go back to state 4 for another byte
case (5):
num_in |= ascii_to_nibble (char_in);
if (p_data != NULL)
{
HXR_DBG (p_serial, hex << p_data << "<-" << num_in << endl);
*p_data++ = num_in;
}
// Update a cyclic reduncancy check (fancy checksum) for the data
computed_crc = _crc_ccitt_update (computed_crc, num_in);
if (--size > 0)
{
decoder_state = 4;
}
else
{
decoder_state = 6;
}
break;
// State 6: Get the first nibble of the checksum
case (6):
received_crc = (uint16_t)(ascii_to_nibble (char_in) << 4);
decoder_state = 7;
break;
// State 7: Get the second nibble of the checksum
case (7):
received_crc |= ascii_to_nibble (char_in);
received_crc <<= 4;
decoder_state = 8;
break;
// State 8: Get the third nibble of the checksum
case (8):
received_crc |= ascii_to_nibble (char_in);
received_crc <<= 4;
decoder_state = 9;
break;
// State 9: Get the last nibble of the checksum and check the CRC
case (9):
received_crc |= ascii_to_nibble (char_in);
decoder_state = 0;
HXR_DBG (p_serial, PMS (":") << received_crc << endl);
if (received_crc == computed_crc)
{
good_data = true;
}
else
{
HXR_DBG (p_serial, endl << PMS ("Bad CRC:") << hex << received_crc
<< PMS (",") << computed_crc << dec << endl);
}
break;
// We should never get here. Wait for a return, then reset
default:
// Code just before the state machine detects reset characters
break;
};
}
return (good_data);
}
/** @brief Program a page.
*
* When using CRC check, the page is now written on mismatch.
*
* Parameters:
* - page address (u32), must be aligned
* - CRC (u16), ignored if CRC check is not available
* - page data
*/
static void cmd_prog_page(void)
{
const addr_type addr = recv_addr();
#ifndef DISABLE_STRICT_CHECKS
// addr aligned on SPM_PAGESIZE and not in the bootloader area
// writing the last page is allowed (to update roblocop)
if( (addr & ((addr_type)SPM_PAGESIZE-1)) != 0 ||
#ifndef DISABLE_COPY_PAGES
(addr > BOOTLOADER_ADDR && addr <= FLASHEND-SPM_PAGESIZE)
#else
addr > BOOTLOADER_ADDR
#endif
) {
reply_error(STATUS_BAD_VALUE);
return;
}
#endif
eeprom_busy_wait();
#ifdef DISABLE_PROG_CRC
recv_u16(); // eat the crc
uint8_t i;
// Read data and fill temporary page buffer
for( i=0; i<SPM_PAGESIZE/2; i++ ) {
boot_page_fill(addr+2*i, recv_u16());
}
#else
const uint16_t crc_expected = recv_u16();
uint16_t crc = 0xffff;
uint8_t i;
// Read data, fill temporary page buffer, compute CRC
for( i=0; i<SPM_PAGESIZE/2; i++ ) {
uint8_t c1 = recv_u8();
uint8_t c2 = recv_u8();
crc = _crc_ccitt_update(crc, c1);
crc = _crc_ccitt_update(crc, c2);
uint16_t w = c1 + (c2<<8); // little endian word
boot_page_fill(addr+2*i, w);
}
// check CRC
if( crc != crc_expected ) {
boot_rww_enable();
reply_error(STATUS_CRC_MISMATCH);
return;
}
#endif
// Erase page
boot_page_erase(addr);
boot_spm_busy_wait();
// Write the page buffer to the page
boot_page_write(addr);
boot_spm_busy_wait();
// Reenable RWW section
boot_rww_enable();
reply_success(0);
}
uint16_t mdata_crc_update(uint16_t crc, uint8_t data)
{
return _crc_ccitt_update(crc, data);
}
uint16_t CrystalFontz::CRC(uint8_t *ptr, uint16_t len)
{
uint16_t crc = 0xFFFF;
while(len--) crc = _crc_ccitt_update(crc, *ptr++);
return(~crc);
}
