void
app_cfg_flush()
{
sxfs_part_id_t used_app_cfg_part = SP_APP_CFG_1;
app_cfg_rec_t* app_cfg = app_cfg_load(&used_app_cfg_part);
chMtxLock(&app_cfg_mtx);
app_cfg_local.crc = crc32_block(0, &app_cfg_local.data, sizeof(app_cfg_data_t));
if (memcmp(&app_cfg_local, app_cfg, sizeof(app_cfg_rec_t)) != 0) {
sxfs_part_id_t unused_app_cfg_part =
(used_app_cfg_part == SP_APP_CFG_1) ? SP_APP_CFG_2 : SP_APP_CFG_1;
bool ret = sxfs_write(unused_app_cfg_part, 0, (uint8_t*)&app_cfg_local, sizeof(app_cfg_local));
if (ret) {
ret = sxfs_erase(used_app_cfg_part);
if (!ret)
printf("app cfg erase failed! %d\r\n", used_app_cfg_part);
}
else {
printf("app cfg write failed! %d\r\n", unused_app_cfg_part);
}
}
chMtxUnlock();
if (app_cfg != NULL)
free(app_cfg);
}
static void end_msg(void)
{
unsigned char pad[ENCR_BLOCK_SIZE - (packet.len - 17 + 4) % ENCR_BLOCK_SIZE];
brandom_fill(pad, sizeof pad);
pkt_add_b(&packet, pad, sizeof pad);
pkt_add_u4(&packet, crc32_block(packet.s+17, packet.len-17));
encr_blocks(&encryptor,
(unsigned char*)packet.s+17, packet.len-17, seq_first);
pkt_add_cc(&packet, &msg_authenticator);
}
static void
boot_app()
{
if (memcmp((const void*)_app_hdr.magic, "BBMT-APP", 8) == 0) {
uint32_t crc_calc = crc32_block(0xffffffff, __app_start__, _app_hdr.img_size) ^ 0xffffffff;
if (crc_calc == _app_hdr.crc) {
chThdSleepMilliseconds(100);
jump_to_app((uint32_t)__app_start__);
}
}
}
static app_cfg_rec_t*
app_cfg_load_from(sxfs_part_id_t part)
{
bool ret;
app_cfg_rec_t* app_cfg = malloc(sizeof(app_cfg_rec_t));
ret = sxfs_read(part, 0, app_cfg, sizeof(app_cfg_rec_t));
if (!ret) {
free(app_cfg);
return NULL;
}
uint32_t calc_crc = crc32_block(0, &app_cfg->data, sizeof(app_cfg_data_t));
if (calc_crc != app_cfg->crc) {
free(app_cfg);
return NULL;
}
return app_cfg;
}
int read_data()
{
unsigned char buffer[BUFFER_SIZE];
oem4_header_t header;
int result, read_len, rd_cnt;
unsigned int crc;
pushback_reader_t efd;
oem4_bestpos_t bestpos;
oem4_bestvel_t bestvel;
gps_pos_novatel_t gps_pos;
gps_vel_novatel_t gps_vel;
struct timespec tloc;
static int is_init = 0;
//in case of read-errors from ser. device this function will be called several times in case of error from main.
//
if(is_init) {
erret(pb_reset(&efd));
}
else {
erret(pb_init(fd, BUFFER_SIZE*2, &efd));
is_init = 1;
}
memset(buffer, 0, sizeof(buffer));
memset(&gps_pos, 0, sizeof(gps_pos));
memset(&gps_vel, 0, sizeof(gps_vel));
rd_cnt = 0;
result = 0;
while(1)
{
// read sync
erret(pb_read(&efd, buffer, (unsigned int *)&read_len, OEM4_SYNC_SIZE));
if(read_len<=0) fprintf(stderr,"read timeout\r\n");
// read enough (3) sync bytes?
if (read_len != OEM4_SYNC_SIZE)
{
// no, pusback what we had read so far and try again...
erret(pb_unread(&efd, buffer, read_len));
continue;
}
// increment counter of read bytes
rd_cnt += read_len;
// sync found?
if (buffer[0] != OEM4_BIN_SYNC_HEADER[0] ||
buffer[1] != OEM4_BIN_SYNC_HEADER[1] ||
buffer[2] != OEM4_BIN_SYNC_HEADER[2])
{
// search for sync message or part of sync message
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
// if no (partial) sync message is found, nothing is unread (rd_cnt - result) == 0
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
// everything is pushed back, therefore nothing is read
rd_cnt = 0;
continue;
}
// read head length
erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, 1));
// nothing was read?
if (result == 0)
{
// search for sync message or part of sync message
result = oem4_search_sync(buffer, rd_cnt);
// push back and try again
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
rd_cnt += read_len;
// assign header length
header.length = buffer[3];
// header too long for the buffer?
if(header.length + OEM4_SYNC_SIZE >= BUFFER_SIZE)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
// read header, keep in mind that four bytes of the header are allready read
erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.length - 4));
// read less than expected?
if (read_len != header.length - 4)
{
// search for sync message or part of sync message
result = oem4_search_sync(buffer, rd_cnt + read_len);
// push back what we had and try again
erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result));
rd_cnt = 0;
continue;
}
rd_cnt += read_len;
oem4_fill_header(&header, buffer);
// data too long for the buffer?
if ((header.message_length + header.length + OEM4_SYNC_SIZE + OEM4_CRC_SIZE) >= BUFFER_SIZE)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
// read data and CRC
erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.message_length + OEM4_CRC_SIZE));
// read less than expected?
if (read_len != header.message_length + OEM4_CRC_SIZE)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt + read_len);
// push back what we had and try again
erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result));
rd_cnt = 0;
continue;
}
rd_cnt += read_len;
crc32_block(buffer, rd_cnt, &crc);
// CRC ok? (crc == 0)
if (crc)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
// decode message
switch(header.message_id)
{
case OEM4_BESTPOS:
if (!oem4_get_best_pos(&bestpos, &buffer[header.length], header.message_length))
{
#if AR_VERBOSE==1
oem4_dump_best_pos(&bestpos);
#endif
memcpy(&bestpos.gps_header, &header, sizeof(header));
create_gps_position(&bestpos, &gps_pos);
SHM_WriteSlot(config.ishm_pos_out, &gps_pos, sizeof(gps_pos));
clock_gettime(CLOCK_REALTIME, &tloc);
if(abs(tloc.tv_sec - gps_pos.gps_time.tv_sec) > 60*60*4 && gps_pos.solution_type>0)
clock_settime(CLOCK_REALTIME, &gps_pos.gps_time);
}
break;
case OEM4_BESTVEL:
if (!oem4_get_best_vel(&bestvel, &buffer[header.length], header.message_length))
{
memcpy(&bestvel.gps_header, &header, sizeof(header));
create_gps_velocity(&bestvel, &gps_vel);
SHM_WriteSlot(config.ishm_vel_out, &gps_vel, sizeof(gps_vel));
}
break;
}
rd_cnt = 0;
}
return 0;
}
static uint32 connection_hash(struct connection_key const* key)
{
return crc32_block((const char*)key, sizeof *key);
}
int cartdb_find(cart_t *cart)
{
node_t *cartnode,*boardnode,*node;
u32 crc32,wramsize,vramsize,battery;
char *str,*str2,*tmp;
if(cartxml == 0)
return(1);
//calculate crc32 of entire image
crc32 = crc32_block(cart->prg.data,cart->prg.size,0);
crc32 = crc32_block(cart->chr.data,cart->chr.size,crc32);
//initialize the size and battery vars
wramsize = vramsize = 0;
battery = 0;
//try to find cart node with same crc32
cartnode = find_cart(crc32);
if(cartnode) {
//copy game title
if((str = find_attrib(cartnode->parent,"name")))
strcpy(cart->title,str);
//see if board node exists
if((boardnode = get_child("board",cartnode))) {
//get unif board name and ines mapper number
str = find_attrib(boardnode,"type");
str2 = find_attrib(boardnode,"mapper");
//set mapperid with information discovered
cart->mapperid = determine_mapperid(cart,str,str2,boardnode);
//find the vram size
node = get_child("vram",boardnode);
while(node) {
tmp = find_attrib(node,"size");
if(tmp) {
battery |= hasbattery(node) << 1;
vramsize += sizestr2int(tmp);
}
node = get_sibling("vram",node);
}
//find the wram size and battery status
node = get_child("wram",boardnode);
while(node) {
tmp = find_attrib(node,"size");
if(tmp) {
battery |= hasbattery(node);
wramsize += sizestr2int(tmp);
}
node = get_sibling("wram",node);
}
//debug messages
if(wramsize) {
cart_setwramsize(cart,wramsize / 1024);
if(battery & 1)
cart->battery |= 1;
}
if(vramsize) {
cart_setvramsize(cart,vramsize / 1024);
if(battery & 2)
cart->battery |= 2;
}
return(0);
}
}
log_printf("cartdb_find: cart not found in database.\n");
return(1);
}
void
app_cfg_init()
{
chMtxInit(&app_cfg_mtx);
app_cfg_rec_t* app_cfg = app_cfg_load(NULL);
if (app_cfg != NULL) {
app_cfg_local = *app_cfg;
app_cfg_local.data.reset_count++;
free(app_cfg);
}
else {
app_cfg_local.data.reset_count = 0;
app_cfg_local.data.temp_unit = UNIT_TEMP_DEG_F;
app_cfg_local.data.control_mode = ON_OFF;
app_cfg_local.data.hysteresis.value = 1;
app_cfg_local.data.hysteresis.unit = UNIT_TEMP_DEG_F;
app_cfg_local.data.net_settings.security_mode = 0;
app_cfg_local.data.net_settings.ip_config = IP_CFG_DHCP;
app_cfg_local.data.net_settings.ip = 0;
app_cfg_local.data.net_settings.subnet_mask = 0;
app_cfg_local.data.net_settings.gateway = 0;
app_cfg_local.data.net_settings.dns_server = 0;
touch_calib_reset();
app_cfg_local.data.controller_settings[CONTROLLER_1].controller = CONTROLLER_1;
app_cfg_local.data.controller_settings[CONTROLLER_1].setpoint_type = SP_STATIC;
app_cfg_local.data.controller_settings[CONTROLLER_1].static_setpoint.value = 68;
app_cfg_local.data.controller_settings[CONTROLLER_1].static_setpoint.unit = UNIT_TEMP_DEG_F;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_1].enabled = false;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_1].function = OUTPUT_FUNC_COOLING;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_1].cycle_delay.unit = UNIT_TIME_MIN;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_1].cycle_delay.value = 3;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_2].enabled = false;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_2].function = OUTPUT_FUNC_HEATING;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_2].cycle_delay.unit = UNIT_TIME_MIN;
app_cfg_local.data.controller_settings[CONTROLLER_1].output_settings[OUTPUT_2].cycle_delay.value = 3;
app_cfg_local.data.controller_settings[CONTROLLER_2].controller = CONTROLLER_2;
app_cfg_local.data.controller_settings[CONTROLLER_2].setpoint_type = SP_STATIC;
app_cfg_local.data.controller_settings[CONTROLLER_2].static_setpoint.value = 68;
app_cfg_local.data.controller_settings[CONTROLLER_2].static_setpoint.unit = UNIT_TEMP_DEG_F;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_1].enabled = false;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_1].function = OUTPUT_FUNC_COOLING;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_1].cycle_delay.unit = UNIT_TIME_MIN;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_1].cycle_delay.value = 3;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_2].enabled = false;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_2].function = OUTPUT_FUNC_HEATING;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_2].cycle_delay.unit = UNIT_TIME_MIN;
app_cfg_local.data.controller_settings[CONTROLLER_2].output_settings[OUTPUT_2].cycle_delay.value = 3;
app_cfg_local.crc = crc32_block(0, &app_cfg_local.data, sizeof(app_cfg_data_t));
}
}