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; } }