void sendResponse(uint16_t response)
{
uint8_t txBuffer[100];
uint8_t* txPtr = txBuffer;
uint8_t packetLength;
uint16_t writePointer;
writePointer = netReadWord(REG_S3_TX_WR0) + S3_TX_START;
switch(response) {
default:
case ERROR_UNKNOWN:
// Send unknown error packet
packetLength = TFTP_UNKNOWN_ERROR_LEN;
memcpy_P(txBuffer, tftp_unknown_error_packet, packetLength);
break;
case ERROR_INVALID:
// Send invalid opcode packet
packetLength = TFTP_OPCODE_ERROR_LEN;
memcpy_P(txBuffer, tftp_opcode_error_packet, packetLength);
break;
case ERROR_FULL:
// Send unknown error packet
packetLength = TFTP_FULL_ERROR_LEN;
memcpy_P(txBuffer, tftp_full_error_packet, packetLength);
break;
case ACK:
if(lastPacket > highPacket) highPacket = lastPacket;
#ifdef _DEBUG_TFTP
traceln("Tftp: Sent ACK");
/* no break */
#endif
case FINAL_ACK:
#ifdef _DEBUG_TFTP
if(response == FINAL_ACK) traceln("Tftp: Sent Final ACK ");
#endif
packetLength = 4;
*txPtr++ = TFTP_OPCODE_ACK >> 8;
*txPtr++ = TFTP_OPCODE_ACK & 0xff;
// lastPacket is block code
*txPtr++ = lastPacket >> 8;
*txPtr = lastPacket & 0xff;
break;
}
txPtr = txBuffer;
while(packetLength--) {
netWriteReg(writePointer++, *txPtr++);
if(writePointer == S3_TX_END) writePointer = S3_TX_START;
}
netWriteWord(REG_S3_TX_WR0, writePointer - S3_TX_START);
netWriteReg(REG_S3_CR, CR_SEND);
while(netReadReg(REG_S3_CR));
#ifdef _VERBOSE
traceln("Tftp: Response sent");
#endif
}
uint8_t validImage(uint8_t *base)
{
/* Check that a jump table is present in the first flash sector */
uint8_t i;
for(i = 0; i < 0x34; i += 4) {
// For each vector, check it is of the form:
// 0x0C 0x94 0xWX 0xYZ ; JMP 0xWXYZ
if(base[i] != 0x0c) {
#ifdef _DEBUG_VALD
traceln("Vald: Validation failed at address ");
tracenum(i);
trace(" with ");
tracenum(base[i]);
trace(" instead of 0x0C");
#endif
return(0);
}
if(base[i + 1] != 0x94) {
#ifdef _DEBUG_VALD
traceln("Vald: Validation failed at address ");
tracenum(i + 1);
trace(" with ");
tracenum(base[i + 1]);
trace(" instead of 0x94");
#endif
return(0);
}
}
#ifdef _DEBUG_VALD
traceln("Vald: Valid image");
#endif
return(1);
}
/**
* Initializes the network controller
*/
void tftpInit()
{
// Open socket
sockInit(TFTP_PORT);
#ifndef _TFTP_RANDOM_PORT
if(eeprom_read_byte(EEPROM_SIG_3) == EEPROM_SIG_3_VALUE)
tftpTransferPort = ((eeprom_read_byte(EEPROM_PORT + 1) << 8) + eeprom_read_byte(EEPROM_PORT));
else
tftpTransferPort = TFTP_STATIC_PORT;
#endif
#ifdef _VERBOSE
traceln("Tftp: TFTP server init done");
#ifndef _TFTP_RANDOM_PORT
traceln("\t Port: ");
tracenum(tftpTransferPort);
#endif
#endif
}
/**
* Initializes the network controller
*/
void tftpInit()
{
// Open socket
do {
// Write TFTP Port
netWriteWord(REG_S3_PORT0, TFTP_PORT);
// Write mode
netWriteReg(REG_S3_MR, MR_UDP);
// Open Socket
netWriteReg(REG_S3_CR, CR_OPEN);
// Read Status
if(netReadReg(REG_S3_SR) != SOCK_UDP)
// Close Socket if it wasn't initialized correctly
netWriteReg(REG_S3_CR, CR_CLOSE);
// If socket correctly opened continue
} while(netReadReg(REG_S3_SR) != SOCK_UDP);
#ifdef _VERBOSE
traceln("Tftp: TFTP server init done");
#endif
}
uint8_t processPacket()
{
#endif
uint8_t buffer[TFTP_PACKET_MAX_SIZE];
uint16_t readPointer;
uint16_t writeAddr;
// Transfer entire packet to RAM
uint8_t *bufPtr = buffer;
uint16_t count;
#ifdef _DEBUG_TFTP
traceln("Tftp: ----");
traceln("Tftp: Starting processing packet of size ");
tracenum(packetSize);
if(packetSize >= 0x800) traceln("Tftp: Overflow");
// step();
#endif
readPointer = netReadWord(REG_S3_RX_RD0);
#ifdef _DEBUG_TFTP
traceln("Tftp: readPointer at position ");
tracenum(readPointer);
#endif
if(readPointer == 0) readPointer += S3_RX_START;
for(count = TFTP_PACKET_MAX_SIZE; count--;) {
#ifdef _DEBUG_TFTP
if((count == TFTP_PACKET_MAX_SIZE - 1) || (count == 0)) {
traceln("Tftp: Reading from position ");
tracenum(readPointer);
}
#endif
*bufPtr++ = netReadReg(readPointer++);
if(readPointer == S3_RX_END) readPointer = S3_RX_START;
}
netWriteWord(REG_S3_RX_RD0, readPointer); // Write back new pointer
netWriteReg(REG_S3_CR, CR_RECV);
while(netReadReg(REG_S3_CR));
#ifdef _DEBUG_TFTP
traceln("Tftp: Bytes left to read ");
tracenum(netReadWord(REG_S3_RX_RSR0));
#endif
#ifdef _DEBUGMORE_TFTP
// Dump packet
bufPtr = buffer;
traceln("");
for(count = TFTP_PACKET_MAX_SIZE / 2; count--;) {
uint16_t val = *bufPtr++;
val |= (*bufPtr++) << 8;
tracenum(val);
if((count % 8) == 0 && count != 0) traceln("");
else trace(" ");
}
#endif
#ifdef _DEBUG_TFTP
traceln("Tftp: Setting return address");
#endif
// Set up return IP address and port
uint8_t i;
for(i = 0; i < 6; i++) netWriteReg(REG_S3_DIPR0 + i, buffer[i]);
// Parse packet
uint16_t tftpDataLen = (buffer[6] << 8) + buffer[7];
uint16_t tftpOpcode = (buffer[8] << 8) + buffer[9];
uint16_t tftpBlock = (buffer[10] << 8) + buffer[11];
#ifdef _DEBUG
traceln("Tftp: This is block ");
tracenum(tftpBlock);
trace(" with opcode ");
tracenum(tftpOpcode);
trace(" and data length ");
tracenum(tftpDataLen - (TFTP_OPCODE_SIZE + TFTP_BLOCKNO_SIZE));
#endif
uint8_t returnCode = ERROR_UNKNOWN;
uint16_t packetLength;
switch(tftpOpcode) {
case TFTP_OPCODE_RRQ: // Read request
#ifdef _DEBUG_TFTP
traceln("Tftp: Read request");
#endif
break;
case TFTP_OPCODE_WRQ: // Write request
#ifdef _VERBOSE
traceln("Tftp: Write request");
#endif
// Flagging image as invalid since the flashing process has started
eeprom_write_byte(EEPROM_IMG_STAT, EEPROM_IMG_BAD_VALUE);
netWriteReg(REG_S3_CR, CR_RECV);
netWriteReg(REG_S3_CR, CR_CLOSE);
do {
netWriteReg(REG_S3_MR, MR_UDP);
netWriteReg(REG_S3_CR, CR_OPEN);
#ifdef _TFTP_RANDOM_PORT
netWriteWord(REG_S3_PORT0, (buffer[4]<<8) | ~buffer[5]); // Generate a 'random' TID (RFC1350)
#else
netWriteWord(REG_S3_PORT0, tftpPort);
#endif
if(netReadReg(REG_S3_SR) != SOCK_UDP)
netWriteReg(REG_S3_CR, CR_CLOSE);
} while(netReadReg(REG_S3_SR) != SOCK_UDP);
#ifdef _DEBUG_TFTP
traceln("Tftp: Changed to port ");
#ifdef _TFTP_RANDOM_PORT
tracenum((buffer[4]<<8) | (buffer[5]^0x55));
#else
tracenum(tftpPort);
#endif
#endif
lastPacket = 0;
returnCode = ACK; // Send back acknowledge for packet 0
break;
case TFTP_OPCODE_DATA:
packetLength = tftpDataLen - (TFTP_OPCODE_SIZE + TFTP_BLOCKNO_SIZE);
lastPacket = tftpBlock;
writeAddr = (tftpBlock - 1) << 9; // Flash write address for this block
#ifdef _VERBOSE
traceln("Tftp: Data for block ");
tracenum(lastPacket);
#endif
if((writeAddr + packetLength) > MAX_ADDR) {
// Flash is full - abort with an error before a bootloader overwrite occurs
// Application is now corrupt, so do not hand over.
#ifdef _VERBOSE
traceln("Tftp: Flash is full");
#endif
returnCode = ERROR_FULL;
} else {
#ifdef _DEBUG_TFTP
traceln("Tftp: Writing data from address ");
tracenum(writeAddr);
#endif
uint8_t *pageBase = buffer + (UDP_HEADER_SIZE + TFTP_OPCODE_SIZE + TFTP_BLOCKNO_SIZE); // Start of block data
uint16_t offset = 0; // Block offset
// Round up packet length to a full flash sector size
while(packetLength % SPM_PAGESIZE) packetLength++;
#ifdef _DEBUG_TFTP
traceln("Tftp: Packet length adjusted to ");
tracenum(packetLength);
#endif
if(writeAddr == 0) {
// First sector - validate
if(!validImage(pageBase)) {
returnCode = INVALID_IMAGE;
/* FIXME: Validity checks. Small programms (under 512 bytes?) don't
* have the the JMP sections and that is why app.bin was failing.
* When flashing big binaries is fixed, uncomment the break below.*/
#ifndef _DEBUG_TFTP
break;
#endif
}
}
// Flash packets
for(offset = 0; offset < packetLength;) {
uint16_t writeValue = (pageBase[offset]) | (pageBase[offset + 1] << 8);
boot_page_fill(writeAddr + offset, writeValue);
#ifdef _DEBUGMORE
if((offset == 0) || ((offset == (packetLength - 2)))) {
traceln("Tftp: Writing ");
tracenum(writeValue);
trace(" at offset ");
tracenum(writeAddr + offset);
}
#endif
offset += 2;
if(offset % SPM_PAGESIZE == 0) {
boot_page_erase(writeAddr + offset - SPM_PAGESIZE);
boot_spm_busy_wait();
boot_page_write(writeAddr + offset - SPM_PAGESIZE);
boot_spm_busy_wait();
boot_rww_enable();
}
}
if(packetLength < TFTP_DATA_SIZE) {
// Flash is complete
// Hand over to application
#ifdef _VERBOSE
traceln("Tftp: Flash is complete");
#endif
// Flag the image as valid since we received the last packet
eeprom_write_byte(EEPROM_IMG_STAT, EEPROM_IMG_OK_VALUE);
returnCode = FINAL_ACK;
} else {
returnCode = ACK;
}
}
break;
// Acknowledgment
case TFTP_OPCODE_ACK:
#ifdef _DEBUG_TFTP
traceln("Tftp: Acknowledge");
#endif
break;
// Error signal
case TFTP_OPCODE_ERROR:
#ifdef _DEBUG_TFTP
traceln("Tftp: Error");
#endif
break;
default:
#ifdef _DEBUG_TFTP
traceln("Tftp: Invalid opcode ");
tracenum(tftpOpcode);
#endif
// Invalid - return error
returnCode = ERROR_INVALID;
break;
}
return(returnCode);
}