// *************************************************************************************************
// @fn db_reset
// @brief reload records from flash to RAM, done at init
// @param none
// @return none
// *************************************************************************************************
void db_reset(void)
{
u8 i;
// Get all records from Flash into RAM
for (i = 0; i < MAX_TABLES; ++i)
{
memcpy(&tables[i], tables_in_flash[i], sizeof(db_table_t));
// crc match?
if (tables[i].crc != crc_compute((u8*)&tables[i], RECORDS_PER_TABLE * sizeof(db_record_t)))
{
memset(&tables[i], 0, sizeof(db_table_t));
}
}
// load info into RAM
memcpy(&info, info_in_flash, sizeof(db_info_t));
if (info.crc != crc_compute((u8*)&info, sizeof(db_info_t) - 2))
{
memset(&info, 0, sizeof(db_info_t));
info.batt_offset = -10;
info.temp_offset = -250;
}
}
// *************************************************************************************************
// @fn db_save
// @brief save a encryption key for a door
// @param id, key
// @return status, 0 = success, 1 = db full
// *************************************************************************************************
u8 db_save(u16 id, u8 key[16])
{
u8 i, j;
u8 empty_found = 0, empty_i = 0, empty_j = 0;
// first find if the record already exists
for (i = 0; i < MAX_TABLES; ++i)
{
for (j = 0; j < RECORDS_PER_TABLE; ++j)
{
if (tables[i].records[j].id == id)
{
// found it
// erase
flash_erase_segment((u8*)tables_in_flash[i]);
// update RAM first
memcpy(tables[i].records[j].key, key, 16);
// calculat new crc
tables[i].crc = crc_compute((u8*)&tables[i], RECORDS_PER_TABLE * sizeof(db_record_t));
// write only one segment
flash_write_segment((u8*)tables_in_flash[i], (u8*)(&tables[i]));
return 0;
}
else if (!empty_found && !tables[i].records[j].id)
{
empty_i = i;
empty_j = j;
empty_found = 1;
}
}
}
// is database all full?
if (!empty_found) return 1;
// erase
flash_erase_segment((u8*)tables_in_flash[empty_i]);
// write to a new slot
tables[empty_i].records[empty_j].id = id;
memcpy(tables[empty_i].records[empty_j].key, key, 16);
// calculat new crc
tables[empty_i].crc = crc_compute((u8*)&tables[empty_i], RECORDS_PER_TABLE * sizeof(db_record_t));
// write only one segment
flash_write_segment((u8*)tables_in_flash[empty_i], (u8*)(&tables[empty_i]));
return 0;
}
/*Send function for MST
* Required args:
* uint8_t fromApp: 1 if sender is source
* uint8_t pkt_type: Type of packet
* uint8_t dest: Final destination of packet
*/
int8_t mst_proto_send(comBuf *pkt, va_list args)
{
uint8_t fromApp;
uint8_t pkt_type;
uint8_t dest;
mst_packet_t *mst_info;
fromApp = va_arg(args, int);
pkt_type = va_arg(args, int);
dest = va_arg(args, int);
if(!fromApp) {
mst_info = (mst_packet_t *)&(pkt->data[pkt->size-sizeof(mst_packet_t)]);
#if defined(MST_USE_CRC)
// compute crc over packet, last two bytes are the crc
mst_s_crc = crc_compute(pkt->data, pkt->size - 2);
mst_info->crc_l = mst_s_crc & 0x00FF;;
mst_info->crc_h = (mst_s_crc & 0xFF00) >> 8;
#endif
#if defined(TESTBED_DIAGNOSTICS)
if(mst_info->type == MST_CONTROL)
com_ioctl_IFACE_RADIO(RADIO_TX_POWER, 0x00);
else if(mst_info->type == MST_DATA)
com_ioctl_IFACE_RADIO(RADIO_TX_POWER, 0x02);
else
printf("unknown packet type!\n");
#endif
//This is a packet in mid path. Just send it off.
return 0;
}
/* Write parameters to eeprom. */
void
eeprom_write_params (void)
{
uint8_t i;
struct eeprom_t p;
/* Prepare parameters. */
p.key = EEPROM_KEY;
p.encoder_right_correction = encoder_right_corrector.correction;
eeprom_write_params_helper (&p, 0, &cs_main.speed_theta,
&cs_main.pos_theta,
&cs_main.blocking_detection_theta,
&output_left);
eeprom_write_params_helper (&p, 1, &cs_main.speed_alpha,
&cs_main.pos_alpha,
&cs_main.blocking_detection_alpha,
&output_right);
#if AC_ASSERV_AUX_NB
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
eeprom_write_params_helper (&p, 2 + i, &cs_aux[i].speed,
&cs_aux[i].pos,
&cs_aux[i].blocking_detection,
&output_aux[i]);
#endif
p.postrack_footing = postrack_footing;
p.traj_eps = traj_eps;
p.traj_aeps = traj_aeps;
p.traj_angle_limit = traj_angle_limit;
p.crc = crc_compute ((uint8_t *) &p, sizeof (p) - 1);
/* Write every sets. */
for (i = 0; i < UTILS_COUNT (eeprom_params); i++)
{
eeprom_write_block (&p, &eeprom_params[i],
sizeof (struct eeprom_t));
}
}
void recv_thread(void)
{
while(1)
{
IF_SET set;
IF_ZERO(&set);
IF_SET(IFACE_RADIO, &set);
if(com_select(&set, 1000))
{
comBuf* p = com_recv(IFACE_RADIO);
if (p && p->data[0] == PKT_DO_SENSE)
{
if (p->size != 5)
continue;
if (crc_compute(p->data, 3) != buf_extract_WORD(p->data, 3))
continue;
send_packet(PKT_REQUESTED_SENSE);
}
com_free_buf(p);
}
}
}
/* Read parameters from eeprom. */
void
eeprom_read_params (void)
{
uint8_t i;
struct eeprom_t loaded;
eeprom_loaded = -1;
/* Load first good set. */
for (i = 0; i < UTILS_COUNT (eeprom_params); i++)
{
/* Read EEPROM. */
eeprom_read_block (&loaded, &eeprom_params[i],
sizeof (struct eeprom_t));
/* Check CRC. */
if (loaded.key == EEPROM_KEY
&& crc_compute ((uint8_t *) &loaded,
sizeof (struct eeprom_t)) == 0)
{
/* Ok. */
eeprom_loaded = i;
break;
}
}
/* Load defaults if no set is good. */
#ifdef EEPROM_DEFAULTS
if (eeprom_loaded == -1)
{
memcpy_P (&loaded, &eeprom_defaults, sizeof (struct eeprom_t));
eeprom_loaded = 0xDF; /* DeFaults. */
}
#endif
if (eeprom_loaded != -1)
{
/* Ok, load parameters. */
encoder_corrector_set_correction (&encoder_right_corrector,
loaded.encoder_right_correction);
eeprom_read_params_helper (&loaded, 0, &cs_main.speed_theta,
&cs_main.pos_theta,
&cs_main.blocking_detection_theta,
&output_left);
eeprom_read_params_helper (&loaded, 1, &cs_main.speed_alpha,
&cs_main.pos_alpha,
&cs_main.blocking_detection_alpha,
&output_right);
#if AC_ASSERV_AUX_NB
for (i = 0; i < AC_ASSERV_AUX_NB; i++)
eeprom_read_params_helper (&loaded, 2 + i, &cs_aux[i].speed,
&cs_aux[i].pos,
&cs_aux[i].blocking_detection,
&output_aux[i]);
#endif
postrack_set_footing (loaded.postrack_footing);
traj_eps = loaded.traj_eps;
traj_aeps = loaded.traj_aeps;
traj_set_angle_limit (loaded.traj_angle_limit);
}
}
u16 db_id(void)
{
if (info.id && info.id != 0xFFFFu)
return info.id;
info.id = 0;
while (!info.id || info.id == 0xFFFFu)
{
info.id ^= TA0R;
info.id ^= crc_compute((u8*)&sDate, sizeof(struct date));
info.id ^= crc_compute((u8*)&sTime, sizeof(struct time));
info.id ^= crc_compute((u8*)&sBatt, sizeof(struct batt));
}
flash_erase_segment((u8*)info_in_flash);
info.crc = crc_compute((u8*)&info, sizeof(db_info_t) - 2);
flash_write_segment((u8*)info_in_flash, (u8*)(&info));
return info.id;
}
void send_packet(uint8_t packet_id)
{
static uint8_t inited = 0;
uint16_t temp;
uint16_t hum;
if (!inited)
{
inited = 1;
dev_open(DEV_MSP_HUMIDITY);
if (dev_mode(DEV_MSP_HUMIDITY, DEV_MODE_ON) == DEV_FAILURE)
{
no_sensor = 1;
}
}
mos_mutex_lock(&send_mutex);
if (no_sensor)
{
temp = rand();
hum = rand();
}
else
{
dev_read(DEV_MSP_TEMPERATURE, &temp, sizeof(temp));
dev_read(DEV_MSP_HUMIDITY, &hum, sizeof(hum));
}
buf.size = 13;
buf.data[0] = packet_id;
buf_insert_WORD(buf.data, 1, mos_node_id_get());
buf_insert_WORD(buf.data, 3, temp);
buf_insert_WORD(buf.data, 5, hum);
if (no_sensor)
{
buf_insert_WORD(buf.data, 7, rand());
buf_insert_WORD(buf.data, 9, rand());
}
else
{
buf_insert_WORD(buf.data, 7, adc_get_conversion16(4));
buf_insert_WORD(buf.data, 9, adc_get_conversion16(5));
}
buf_insert_WORD(buf.data, 11, crc_compute(buf.data, 11));
com_send(IFACE_RADIO, &buf);
mos_mutex_unlock(&send_mutex);
}
void start(void)
{
com_mode(IFACE_RADIO, IF_LISTEN);
/// Send a HELLO packet //////////////////////////////
buf.size = 5;
buf.data[0] = PKT_HELLO;
buf_insert_WORD(buf.data, 1, mos_node_id_get());
buf_insert_WORD(buf.data, 3, crc_compute(buf.data, 3));
//////////////////////////////////////////////////////
com_send(IFACE_RADIO, &buf);
mos_thread_new(sense_thread, 256, PRIORITY_NORMAL);
mos_thread_new(recv_thread, 256, PRIORITY_NORMAL);
}
void print_crc()
{
uint16_t crc;
//com_mode(IFACE_TERMINAL, IF_LISTEN);
while(1)
{
//printf("Enter a string to compute its CRC16-CCITT:\n");
//buf = com_recv(IFACE_TERMINAL);
crc = crc_compute(crc_str, sizeof(crc_str));
printf("CRC: %d\n\n", crc);
com_free_buf(buf);
}
}
inline uint32_t crc32_compute(uint8_t *buffer, const uint16_t length) {
return crc_compute(buffer,
length,
CRCCU_MR_PTYPE_CCIT8023);
}
inline uint16_t crc16_compute(uint8_t *buffer, const uint16_t length) {
return crc_compute(buffer,
length,
CRCCU_MR_PTYPE_CCIT16) & 0xFFFF;
}
static mos_file* reprogram_iface (uint8_t iface, uint32_t* mismatches)
{
uint8_t j;
uint16_t crc, crc_index;
uint32_t addr;
uint32_t image_size;
uint16_t page_index;
uint32_t image_index;
uint8_t size = 0;
uint8_t packet_size;
send_byte_command (iface, ACK);
recvd = com_recv (iface);
if(recvd->data[0] == LOAD_IMAGE) {
image_size = (uint32_t)*(uint16_t *)(&recvd->data[1]);
com_free_buf (recvd);
} else {
com_free_buf (recvd);
printf ("Didn't get LOAD_IMAGE\n");
return 0;
}
// Find the next page after the last program image we wrote
/*uint32_t start_page = (cb->start_addr + cb->byte_count +
ATMEL_FLASH_PAGE_SIZE-1) / ATMEL_FLASH_PAGE_SIZE;
addr = start_page * ATMEL_FLASH_PAGE_SIZE;
if (!compare && addr + image_size < ATMEL_FLASH_SIZE) {
cb->start_addr = addr;
} else if (!compare) {
// There is not enough room on flash to store this image,
// start again at 0
cb->start_addr = addr = 0;
} else {
// We're only comparing, use the address already in the control block
addr = cb->start_addr;
}*/
mos_file* file = NULL;
if (mismatches == NULL) {
file = mos_file_create("prg", image_size);
addr = 0;
} else {
mismatches = 0;
file = mos_file_open("prg");
// file doesn't exist
if (!file) return NULL;
// TODO check for matching file size
addr = file->start;
}
image_index = 0; //image index is how many bytes written from the image
//image size is the total number of bytes in the image
send_byte_command(iface, LOAD_ACK);
while(image_index < image_size) {
mos_led_toggle (0);
recvd = com_recv(iface);
// don't send address for performance
//addr = *(uint16_t *)(&recvd->data[0]);
//addr = image_index;
crc = *(uint16_t *)(&recvd->data[0]);
com_free_buf (recvd);
// Now get the data packets
//page index is how many bytes from the current page we've received
page_index = 0; //page size is the size of the pages in memory.
while (page_index < PAGE_SIZE_BYTES) { //loop until we have a full page
recvd = com_recv(iface);
for (j = 0; j < recvd->size; j++) { //loop through the packet
//copy packet into page buffer [at right index]
pagebuf[page_index+j] = recvd->data[j];
}
packet_size = recvd->size;
com_free_buf (recvd);
page_index += packet_size;
//don't send page ack for performance
//PAGE ACK
//send_byte_command(PACKET_ACK);
// If this is the last packet then we break out
if(packet_size < COM_DATA_SIZE) {
size = packet_size;
break;
}
}
// actually write the page
//dev_ioctl(DEV_ATMEL_FLASH, DEV_SEEK, addr);
if (mismatches == NULL) {
mos_file_write(pagebuf, file, addr, sizeof(pagebuf));
//dev_write(DEV_ATMEL_FLASH, pagebuf, sizeof(pagebuf));
} else {
dev_ioctl(DEV_ATMEL_FLASH, DEV_SEEK, addr);
// Don't write, just compare the buffer with the page already in flash
if (atmel_flash_compare(pagebuf, sizeof(pagebuf)))
(*mismatches)++;
}
addr += sizeof(pagebuf);
// find out the proper index to pass to crc_compute
if (packet_size < COM_DATA_SIZE)
crc_index = (uint16_t)(image_size - image_index);
else
crc_index = page_index;
// perform the crc calculation and send off the result
if (crc_compute (pagebuf, crc_index) != crc)
send_byte_command (iface, CRC_ERROR);
else
send_byte_command (iface, CRC_OK);
// finally update the image index
image_index += page_index;
}
send_byte_command (iface, IMAGE_ACK);
// In case the flash driver is in buffered mode, flush the buffer
dev_ioctl(DEV_ATMEL_FLASH, DEV_FLUSH);
return file;
}
/*
* NAME: tag->parse()
* DESCRIPTION: parse Xing/LAME tag(s)
*/
int tag_parse(struct tag *tag, struct mad_stream const *stream)
{
struct mad_bitptr ptr = stream->anc_ptr;
struct mad_bitptr start = ptr;
unsigned int bitlen = stream->anc_bitlen;
unsigned long magic;
int i;
if (bitlen < 32)
return -1;
magic = mad_bit_read(&ptr, 32);
bitlen -= 32;
if (magic != XING_MAGIC &&
magic != INFO_MAGIC &&
magic != LAME_MAGIC) {
/*
* Due to an unfortunate historical accident, a Xing VBR tag may be
* misplaced in a stream with CRC protection. We check for this by
* assuming the tag began two octets prior and the high bits of the
* following flags field are always zero.
*/
if (magic != ((XING_MAGIC << 16) & 0xffffffffL) &&
magic != ((INFO_MAGIC << 16) & 0xffffffffL))
return -1;
magic >>= 16;
/* backtrack the bit pointer */
ptr = start;
mad_bit_skip(&ptr, 16);
bitlen += 16;
}
if ((magic & 0x0000ffffL) == (XING_MAGIC & 0x0000ffffL))
tag->flags |= TAG_VBR;
/* Xing tag */
if (magic == LAME_MAGIC) {
ptr = start;
bitlen += 32;
}
else if (parse_xing(&tag->xing, &ptr, &bitlen) == 0)
tag->flags |= TAG_XING;
/* encoder string */
if (bitlen >= 20 * 8) {
start = ptr;
for (i = 0; i < 20; ++i) {
tag->encoder[i] = mad_bit_read(&ptr, 8);
if (tag->encoder[i] == 0)
break;
/* keep only printable ASCII chars */
if (tag->encoder[i] < 0x20 || tag->encoder[i] >= 0x7f) {
tag->encoder[i] = 0;
break;
}
}
tag->encoder[20] = 0;
ptr = start;
}
/* LAME tag */
if (stream->next_frame - stream->this_frame >= 192 &&
parse_lame(&tag->lame, &ptr, &bitlen,
crc_compute(stream->this_frame, 190, 0x0000)) == 0) {
tag->flags |= TAG_LAME;
tag->encoder[9] = 0;
}
else {
for (i = 0; i < 20; ++i) {
if (tag->encoder[i] == 0)
break;
/* stop at padding chars */
if (tag->encoder[i] == 0x55) {
tag->encoder[i] = 0;
break;
}
}
}
return 0;
}
