int FS460_image_set_start_write(const void *p_image_data, unsigned long length)
{
int err;
void *p_buffer;
if (!p_image_data)
return FS460_ERR_INVALID_PARAMETER;
// allocate a buffer for length and data bytes
p_buffer = OS_alloc(sizeof(unsigned long) + length);
if (!p_buffer)
return FS460_ERR_INSUFFICIENT_MEMORY;
// store length and data bytes
*(unsigned long *)p_buffer = length;
OS_memcpy(((unsigned long *)p_buffer) + 1, p_image_data, length);
// call driver
err = send_io_code_to_driver(IOC_IMAGE_SET_START_WRITE, p_buffer, sizeof(unsigned long) + length);
// free buffer
OS_free(p_buffer);
return err;
}
void flashstore_compact(unsigned char len, unsigned char* tempmemstart, unsigned char* tempmemend)
{
// Need at least FLASHSTORE_PAGESIZE
if (tempmemend - tempmemstart < FLASHSTORE_PAGESIZE)
{
return;
}
// Find the lowest age page which this will fit in.
unsigned char pg;
unsigned char selected = 0;
flashpage_age age = 0xFFFFFFFF;
for (pg = 0; pg < FLASHSTORE_NRPAGES; pg++)
{
flashpage_age cage = *(flashpage_age*)FLASHSTORE_PAGEBASE(pg);
if (cage < age && orderedpages[pg].waste + orderedpages[pg].free >= len)
{
selected = pg;
age = cage;
}
}
if (age != 0xFFFFFFFF)
{
// Found enough space for the line, compact the page
// Copy the page into RAM
unsigned char* ram = tempmemstart;
unsigned char* flash = (unsigned char*)FLASHSTORE_PAGEBASE(selected);
OS_memcpy(ram, flash, FLASHSTORE_PAGESIZE);
// Erase the page
OS_flashstore_erase(FLASHSTORE_FPAGE(flash));
OS_flashstore_write(FLASHSTORE_FADDR(flash), (unsigned char*)&lastage, FLASHSTORE_WORDS(sizeof(lastage)));
lastage++;
orderedpages[selected].waste = 0;
orderedpages[selected].free = FLASHSTORE_PAGESIZE - sizeof(flashpage_age);
// Copy the old lines back in. More efficient ways to do this, but okay for the moment
unsigned char* ptr;
for (ptr = ram + sizeof(flashpage_age); ptr < ram + FLASHSTORE_PAGESIZE; )
{
unsigned short id = *(unsigned short*)ptr;
unsigned char len = FLASHSTORE_PADDEDSIZE(ptr[sizeof(unsigned short)]);
if (id == FLASHID_FREE)
{
break;
}
else if (id != FLASHID_INVALID)
{
OS_flashstore_write(FLASHSTORE_FADDR(ptr - ram + flash), ptr, FLASHSTORE_WORDS(len));
orderedpages[selected].free -= len;
}
ptr += len;
}
// We corrupted memory, so we need to reinitialize
flashstore_init((unsigned char**)lineindexstart);
}
}
unsigned char osal_snv_read(unsigned char id, unsigned char len, void *pBuf)
{
unsigned char* mem = flashstore_findspecial(FLASHSPECIAL_SNV + id);
if (mem && mem[FLASHSPECIAL_DATA_LEN] == len)
{
OS_memcpy(pBuf, mem + FLASHSPECIAL_DATA_OFFSET, len);
return 1;
}
else
{
return 0;
}
}
void Motor_run()
{
if(MPI_check_available(MOTOR_DRIVER_PID) == true)
{
MPI_get_message(MOTOR_DRIVER_PID, &incomingPID, &cmdSize, incomingMsg);
if(cmdSize == sizeof(int))
{
OS_memcpy(&incomingCmd, incomingMsg, cmdSize);
switch(incomingCmd)
{
case C_DRIVE_FORWARD:
UART_send("drive forward\r\n", CONSOLE_BASE);
driveForward();
break;
case C_DRIVE_REVERSE:
UART_send("drive reverse\r\n", CONSOLE_BASE);
driveReverse();
break;
case C_DRIVE_LEFT:
UART_send("drive left\r\n", CONSOLE_BASE);
driveLeft();
break;
case C_DRIVE_RIGHT:
UART_send("drive right\r\n", CONSOLE_BASE);
driveRight();
break;
case C_DRIVE_HALT:
UART_send("drive halt\r\n", CONSOLE_BASE);
driveHalt();
break;
}
}
}
}
//
// Invalidate the line entry at the given address.
//
static void flashstore_invalidate(unsigned short* mem)
{
struct
{
unsigned short invalid;
unsigned char len;
unsigned char padding;
} invalid;
OS_memcpy(&invalid, mem, sizeof(invalid));
invalid.invalid = FLASHID_INVALID;
OS_flashstore_write(FLASHSTORE_FADDR(mem), (unsigned char*)&invalid, FLASHSTORE_WORDS(sizeof(invalid)));
orderedpages[((unsigned char*)mem - flashstore) / FLASHSTORE_PAGESIZE].waste += invalid.len;
}
unsigned char osal_snv_write(unsigned char id, unsigned char len, void *pBuf)
{
if (heap + len + FLASHSPECIAL_DATA_OFFSET > sp)
{
return 0;
}
else
{
unsigned char* item = heap;
heap += len + FLASHSPECIAL_DATA_OFFSET;
*(unsigned long*)&item[FLASHSPECIAL_ITEM_ID] = FLASHSPECIAL_SNV + id;
item[FLASHSPECIAL_DATA_LEN] = len;
OS_memcpy(item + FLASHSPECIAL_DATA_OFFSET, pBuf, len);
unsigned char r = flashstore_addspecial(item);
heap = item;
return r;
}
}
