void tftp_handle_packet(void) { #ifdef SKIPJACK_SUPPORT unsigned char key[10] = CONF_TFTP_KEY; #endif /* * overwrite udp connection information (i.e. take from incoming packet) */ uip_ipaddr_copy(uip_udp_conn->ripaddr, BUF->srcipaddr); uip_udp_conn->rport = BUF->srcport; /* * care for incoming tftp packet now ... */ uint16_t i, base; struct tftp_hdr *pk = uip_appdata; switch(HTONS(pk->type)) { #ifndef TFTP_UPLOAD_ONLY /* * streaming data back to the client (download) ... */ case 1: /* read request */ uip_udp_conn->appstate.tftp.download = 1; uip_udp_conn->appstate.tftp.transfered = 0; uip_udp_conn->appstate.tftp.finished = 0; bootload_delay = 0; /* Stop bootloader. */ goto send_data; case 4: /* acknowledgement */ if(uip_udp_conn->appstate.tftp.download != 1) goto error_out; if(HTONS(pk->u.ack.block) < uip_udp_conn->appstate.tftp.transfered || (HTONS(pk->u.ack.block) > uip_udp_conn->appstate.tftp.transfered + 1)) goto error_out; /* ack out of order */ uip_udp_conn->appstate.tftp.transfered = HTONS(pk->u.ack.block); send_data: if(uip_udp_conn->appstate.tftp.finished) { bootload_delay = CONF_BOOTLOAD_DELAY; /* Restart bootloader. */ return; /* nothing more to do */ } pk->type = HTONS(3); /* data packet */ pk->u.data.block = HTONS(uip_udp_conn->appstate.tftp.transfered + 1); base = 512 * uip_udp_conn->appstate.tftp.transfered; #if FLASHEND == 0xFFFF if(uip_udp_conn->appstate.tftp.transfered && base == 0) /* base overflowed ! */ #else if(base > FLASHEND) #endif { uip_udp_send(4); /* send empty packet to finish transfer */ uip_udp_conn->appstate.tftp.finished = 1; return; } for(i = 0; i < 512; i ++) #if defined(SKIPJACK_SUPPORT) pk->u.data.data[i + 8] = #else pk->u.data.data[i] = #endif pgm_read_byte_near(base + i); #if defined(SKIPJACK_SUPPORT) for(i = 0; i < 8; i ++) { /* prepend 8 bytes IV */ pk->u.data.data[i] = rand() & 0xFF; /* prepare to append CBC-MAC, i.e. initialize to zero, * so we can XOR the CBC carry and afterwards encrypt. */ pk->u.data.data[i + 512 + 8] = 0; } /* perform skipjack-cbc encryption: * leave the first block (iv) untouched, * encrypt blocks 1..64 (data) * then encrypt block 65 (cbc-mac) */ for(i = 1; i <= 65; i ++) { /* carry cbc forward (xor) */ for(int j = 0; j < 8; j ++) { pk->u.data.data[(i << 3) + j] ^= pk->u.data.data[(i << 3) + j - 8]; } /* encrypt data */ skipjack_enc(&pk->u.data.data[i << 3], key); } uip_udp_send(4 + 16 + 512); #else /* !SKIPJACK_SUPPORT */ uip_udp_send(4 + 512); #endif uip_udp_conn->appstate.tftp.transfered ++; break; #endif /* not TFTP_UPLOAD_ONLY */ /* * streaming data from the client (firmware upload) ... */ case 2: /* write request */ uip_udp_conn->appstate.tftp.download = 0; uip_udp_conn->appstate.tftp.transfered = 0; uip_udp_conn->appstate.tftp.finished = 0; pk->u.ack.block = HTONS(0); goto send_ack; case 3: /* data packet */ bootload_delay = 0; /* Stop bootloader. */ if(uip_udp_conn->appstate.tftp.download != 0) goto error_out; if(HTONS(pk->u.ack.block) < uip_udp_conn->appstate.tftp.transfered) goto error_out; /* too early */ if(HTONS(pk->u.ack.block) == uip_udp_conn->appstate.tftp.transfered) goto send_ack; /* already handled */ if(HTONS(pk->u.ack.block) > uip_udp_conn->appstate.tftp.transfered + 1) goto error_out; /* too late */ #ifdef SKIPJACK_SUPPORT /* the packet looks like this: * 0..3 <packet preamble> * 4..11 IV <- pk->u.data.data[0] * 12..522 DATA (maybe shorter) --,- "decrypted" * 523..530 CBC-MAC --' */ for(i = 1; i < (uip_datalen() - 4) >> 3; i ++) { /* decrypt block (we actually encrypted, since used decrypt) */ unsigned char buf[8]; memmove(buf, pk->u.data.data + (i << 3), 8); skipjack_enc(buf, key); /* now XOR the decrypted data onto the block before, * which is still encrypted, i.e. our "cbc carry" */ for(int j = 0; j < 8; j ++) pk->u.data.data[(i << 3) + j - 8] ^= buf[j]; } uip_datalen() -= 16; /* lie about packet len */ /* verify our CBC-MAC (XOR'd to zeroes), which should reside right * after the last datablock, which we've moved 8 bytes to the left */ for(i = 0; i < 8; i ++) if(pk->u.data.data[i + uip_datalen() - 4]) goto error_out; #endif /* SKIPJACK_SUPPORT */ base = 512 * (HTONS(pk->u.ack.block) - 1); for(i = uip_datalen() - 4; i < 512; i ++) pk->u.data.data[i] = 0xFF; /* EOF reached, init rest */ for(i = 0; i < 512 / SPM_PAGESIZE; i ++) flash_page(base + i * SPM_PAGESIZE, pk->u.data.data + i * SPM_PAGESIZE); if(uip_datalen() < 512 + 4) { uip_udp_conn->appstate.tftp.finished = 1; # ifdef TFTPOMATIC_SUPPORT bootload_delay = 1; /* ack, then start app */ # else bootload_delay = CONF_BOOTLOAD_DELAY; /* Restart bootloader. */ # endif } uip_udp_conn->appstate.tftp.transfered = HTONS(pk->u.ack.block); send_ack: pk->type = HTONS(4); uip_udp_send(4); /* send ack */ break; /* * protocol errors */ error_out: case 5: /* error */ default: pk->type = HTONS(5); /* data packet */ pk->u.error.code = HTONS(0); /* undefined error code */ pk->u.error.msg[0] = 0; /* yes, really expressive */ uip_udp_send(5); break; } }
BOOL flash_module(WORD page, WORD numpages, char *fn, char *name, BYTE flags) { int i; static WORD bytes_read; static WORD b, last_b = 0; static long addr; static WORD pages; pages = numpages; if(flags & 0x01) { prog_fres = f_open(&prog_file, fn, FA_READ); if(prog_fres == FR_OK) { printf("Flashing %s...", name); i = page; do { MAPPER_MAP1 = 0; FillPage(0xff, 0); prog_fres = f_read(&prog_file, (void *)0x4000, 8192, &bytes_read); addr = (((long)(i & 0x7FF)) << 13); b = flash_addr2block(addr); if(b != last_b) { if(!(flags & 0x04)) { flash_erase_block(b); } last_b = b; } flash_page(0, i); i++; pages--; } while((bytes_read == 8192)&&(pages)); f_close(&prog_file); printf(" OK!\n"); } else { printf("\nCouldn't open %s:\n-> Not flashed!\n", fn); return FALSE; } } pages = numpages; if(flags & 0x02) { prog_fres = f_open(&prog_file, fn, FA_READ); if(prog_fres == FR_OK) { printf("Verifying %s...", name); i = page; do { MAPPER_MAP1 = 0; FillPage(0xff, 0); prog_fres = f_read(&prog_file, (void *)0x4000, 8192, &bytes_read); // if(bytes_read != 0x2000) { // printf("Can't read 8192 bytes from file, but %d (error %d)\n", bytes_read, prog_fres); // } if(b=verify_page(0, i)) { // return 0 on good, address when wrong printf(" ERROR!\n",b); dump_hex((void*)(b&0xFFF0), 16); b ^= 0x2000; dump_hex((void*)(b&0xFFF0), 16); return FALSE; } i++; pages--; } while((bytes_read == 8192)&&(pages)); f_close(&prog_file); printf(" OK!\n"); } else { printf("\nCouldn't open %s:\n-> Not verified!\n", fn); return FALSE; } } return TRUE; }
void tftp_handle_packet(void) { /* * overwrite udp connection information (i.e. take from incoming packet) */ uip_ipaddr_copy(uip_udp_conn->ripaddr, BUF->srcipaddr); uip_udp_conn->rport = BUF->srcport; /* * care for incoming tftp packet now ... */ uint16_t i; flash_base_t base; struct tftp_hdr *pk = uip_appdata; switch(HTONS(pk->type)) { #ifndef TFTP_UPLOAD_ONLY /* * streaming data back to the client (download) ... */ case 1: /* read request */ uip_udp_conn->appstate.tftp.download = 1; uip_udp_conn->appstate.tftp.transfered = 0; uip_udp_conn->appstate.tftp.finished = 0; bootload_delay = 0; /* Stop bootloader. */ goto send_data; case 4: /* acknowledgement */ if(uip_udp_conn->appstate.tftp.download != 1) goto error_out; if(HTONS(pk->u.ack.block) < uip_udp_conn->appstate.tftp.transfered || (HTONS(pk->u.ack.block) > uip_udp_conn->appstate.tftp.transfered + 1)) goto error_out; /* ack out of order */ uip_udp_conn->appstate.tftp.transfered = HTONS(pk->u.ack.block); send_data: if(uip_udp_conn->appstate.tftp.finished) { bootload_delay = CONF_BOOTLOAD_DELAY; /* Restart bootloader. */ return; /* nothing more to do */ } pk->type = HTONS(3); /* data packet */ pk->u.data.block = HTONS(uip_udp_conn->appstate.tftp.transfered + 1); base = TFTP_BLOCK_SIZE * uip_udp_conn->appstate.tftp.transfered; /* base overflowed ! */ #if FLASHEND == UINT16_MAX if(uip_udp_conn->appstate.tftp.transfered && base == 0) #else if(base > FLASHEND) #endif { uip_udp_send(4); /* send empty packet to finish transfer */ uip_udp_conn->appstate.tftp.finished = 1; return; } for (i = 0; i < TFTP_BLOCK_SIZE; i++) pk->u.data.data[i] = __pgm_read_byte (base + i); uip_udp_send(4 + TFTP_BLOCK_SIZE); uip_udp_conn->appstate.tftp.transfered ++; break; #endif /* not TFTP_UPLOAD_ONLY */ /* * streaming data from the client (firmware upload) ... */ case 2: /* write request */ uip_udp_conn->appstate.tftp.download = 0; uip_udp_conn->appstate.tftp.transfered = 0; uip_udp_conn->appstate.tftp.finished = 0; pk->u.ack.block = HTONS(0); goto send_ack; case 3: /* data packet */ bootload_delay = 0; /* Stop bootloader. */ if(uip_udp_conn->appstate.tftp.download != 0) goto error_out; if(HTONS(pk->u.ack.block) < uip_udp_conn->appstate.tftp.transfered) goto error_out; /* too early */ if(HTONS(pk->u.ack.block) == uip_udp_conn->appstate.tftp.transfered) goto send_ack; /* already handled */ if(HTONS(pk->u.ack.block) > uip_udp_conn->appstate.tftp.transfered + 1) goto error_out; /* too late */ base = TFTP_BLOCK_SIZE * (HTONS(pk->u.ack.block) - 1); for(i = uip_datalen() - 4; i < TFTP_BLOCK_SIZE; i ++) pk->u.data.data[i] = 0xFF; /* EOF reached, init rest */ debug_putchar('.'); for(i = 0; i < TFTP_BLOCK_SIZE / SPM_PAGESIZE; i ++) flash_page(base + i * SPM_PAGESIZE, pk->u.data.data + i * SPM_PAGESIZE); if(uip_datalen() < TFTP_BLOCK_SIZE + 4) { uip_udp_conn->appstate.tftp.finished = 1; # ifdef TFTPOMATIC_SUPPORT bootload_delay = 1; /* ack, then start app */ # else bootload_delay = CONF_BOOTLOAD_DELAY; /* Restart bootloader. */ # endif debug_putstr("end\n"); } uip_udp_conn->appstate.tftp.transfered = HTONS(pk->u.ack.block); send_ack: pk->type = HTONS(4); uip_udp_send(4); /* send ack */ break; /* * protocol errors */ error_out: case 5: /* error */ default: pk->type = HTONS(5); /* data packet */ pk->u.error.code = HTONS(0); /* undefined error code */ pk->u.error.msg[0] = 0; /* yes, really expressive */ uip_udp_send(5); break; } }