msg_t FsWriterThread::main() {
BaseSequentialStream *chp = (BaseSequentialStream*)&SD1;
chprintf(chp, "Filesystem thread online.\r\n");
while (!fs.connect()) chThdSleepMilliseconds(10);
while (!fs.mount()) chThdSleepMilliseconds(10);
while (!fs.openNew()) chThdSleepMilliseconds(10);
fsReady = true;
while(true) {
uint32_t count = buf.count;
if (count > 3000) count = 3000 + 0.0625*(count-3000);
rb_remove(&buf, count, writebuf);
fs.write(writebuf, count);
fs.sync();
if (fsInfoMessageStream.ready()) {
protocol::message::fs_info_message_t m;
m.time = ST2MS(chibios_rt::System::getTime());
fs.getFn(m.fname);
m.fsize = fs.getFileSize();
fsInfoMessageStream.publish(m);
}
yield();
}
// Should never get here
fs.umount();
fs.disconnect();
}
int Write(unsigned long *random_seed, param_t *params)
{
long int_value;
void *value;
int_value = (get_random(random_seed) % params->size) + 1;
// make sure we have an odd value
int_value |= 0x0001;
value = rb_remove(My_Tree, int_value);
if (value == NULL)
{
printf("Failure to remove %ld\n", int_value);
exit(-2);
}
if (!rb_insert(My_Tree, int_value, (void *)int_value) )
{
printf("Failure to insert %ld\n", int_value);
exit(-3);
}
return 0;
}
int Write(unsigned long *random_seed, param_t *params)
{
int errors = 0;
int write_elem;
long int_value;
void *value;
write_elem = get_random(random_seed) % params->size;
#ifdef DEBUG
check_tree();
printf("Remove %ld\n", Values[write_elem]);
#endif
value = rb_remove(My_Tree, Values[write_elem]);
if (value == NULL) errors++;
int_value = get_random(random_seed) % params->scale + 1;
#ifdef DEBUG
check_tree();
printf("Insert %ld\n", int_value);
#endif
while ( !rb_insert(My_Tree, int_value, (void *)int_value) )
{
int_value = get_random(random_seed) % params->scale + 1;
#ifdef DEBUG
printf("Insert %ld\n", int_value);
#endif
}
Values[write_elem] = int_value;
return errors;
}
int main() {
RB_TREE root = NULL;
RB_TYPE rbt;
int v,i;
rb_type_create( &rbt, sizeof(VNODE), 0, cmp, NULL, NULL );
srand(2);
for( i=0; i<1000; i++ ) {
v = rand();
//v = 1000000-i;
rb_insert( &rbt, &root, &v );
}
rb_assert(&rbt,root);
printf( "Tree has %d elements\n", rb_size(&root) );
srand(2);
for( i=0; i<1000; i++ ) {
v = rand();
//v = 1000000-i;
rb_remove( &rbt, &root, &v );
}
rb_assert(&rbt,root);
printf( "Tree has %d elements\n", rb_size(&root) );
rb_free(&rbt,&root);
return 0;
}
int usb_read(void * buf, unsigned int nbytes)
{
U16 toread = nbytes;
U16 received;
U8 *buffer8 = (U8 *)buf;
if (usb_ready == 0)
{
return 0;
}
received = 0;
if (!rxfifo)
{
return 0;
}
// blocking mode
while(toread)
{
/* get characters from ring buffer */
if (!rb_is_empty(rxfifo))
{
*buffer8 = rb_remove(rxfifo);
received++;
buffer8++;
toread--;
}
}
return received;
}
static inline void usart_tx_irq(const usart_dev *dev) {
#ifdef ISR_PERF
uint32_t t=stopwatch_getticks();
#endif
/* Check USART Transmit Data Register Empty Interrupt */
uint16_t sr = dev->USARTx->SR;
if( (sr & USART_F_TXE) && (dev->USARTx->CR1 & USART_MASK_TXEIE) ){
if (dev->txrb && !rb_is_empty(dev->txrb)) {
dev->USARTx->DR = rb_remove(dev->txrb);
dev->state->txbusy = 1;
} else {
/* Disable the USART Transmit Data Register Empty Interrupt */
dev->USARTx->CR1 &= ~USART_MASK_TXEIE;
dev->state->txbusy = 0;
// nops needed to deactivate the irq before irq handler is left
asm volatile("nop");
asm volatile("nop");
}
}
#ifdef ISR_PERF
t = stopwatch_getticks() - t;
isr_time += t;
if(t>max_isr_time) max_isr_time=t;
#endif
}
void print_rb_contents() {
uint16 count = rb_full_count(rb);
SerialUSB.print("ring buffer contents: ");
for (uint16 i = 0; i < count; i++) {
SerialUSB.print((int)rb_remove(rb));
if (i < count - 1) SerialUSB.print(", ");
}
SerialUSB.println();
}
/*
This function will return:
0 - if failed
1 - if fmap was removed without IO
2 - if fmap removal begun and has IO (i.e async)
*/
int vmm_fmap_release_mreg(struct pm_task *task, struct vmm_memory_region *mreg)
{
struct vmm_fmap_descriptor *fmd = (struct vmm_fmap_descriptor*)mreg->descriptor;
if(fmd->status == VMM_FMAP_FLUSHING) // since we only allow one command on the task command
// queue, there cannot be a flush command executing..
// then. the task is dying.
{
// fmap is flushing.. we must wait until it completes
// but let's tell the current action we intend to close the FMAP
fmd->status = VMM_FMAP_CLOSING_RELEASE;
return 0;
}
else if(fmd->status != VMM_FMAP_ACTIVE)
{
return 0;
}
/* Begin removal only if references is 1 */
fmd->references--;
/* Unmap Pages from task address space */
UINT32 addr = (UINT32)mreg->tsk_node.low;
for(; addr < (UINT32)mreg->tsk_node.high; addr += 0x1000)
{
page_out(task->id, (ADDR)addr, 2);
}
/* Remove memory region from trees */
ma_remove(&task->vmm_info.regions, &mreg->tsk_node);
rb_remove(&task->vmm_info.regions_id, &mreg->tsk_id_node);
fmd->regions = fmd->regions->next;
if(fmd->regions)
fmd->regions->prev = NULL;
kfree(mreg);
if(fmd->references == 0)
{
/* Set FMAP as shutting down */
fmd->status = VMM_FMAP_CLOSING;
/* Write all dirty pages */
fmd->io_finished.params[0] = 0; // 0 will indicate we are closing and not flushing
fmd->io_finished.callback = vmm_fmap_flush_callback;
fmd->release_addr = (UINT32)mreg->tsk_node.low - 0x1000;
fmd->creating_task = task->id;
vmm_fmap_flush_callback(&fmd->iosrc, IO_RET_OK);
return 2;
}
return 1;
}
INT32 vmm_fmap_realeased_callback(struct fsio_event_source *iosrc, INT32 ioret)
{
struct vmm_fmap_descriptor *fm = MEMREGNODE2FMAPDESC(rb_search(&vmm.fmap_descriptors, iosrc->id));
struct pm_task *task = tsk_get(fm->creating_task);
struct vmm_memory_region *mreg;
if(task == NULL) return 0;
mreg = vmm_region_get_bydesc(task, (struct vmm_descriptor*)fm);
if(mreg == NULL) return 0;
if(ioret == IO_RET_OK)
{
/* Remove memory region from trees */
ma_remove(&task->vmm_info.regions, &mreg->tsk_node);
rb_remove(&task->vmm_info.regions_id, &mreg->tsk_id_node);
kfree(mreg);
/* Remove FMAP descriptor from global list */
rb_remove(&vmm.fmap_descriptors, &fm->gnode);
/* Free structures */
kfree(fm);
kfree(mreg);
/* Invoke command module callback */
if(task->command_inf.callback != NULL)
task->command_inf.callback(task, ioret, 0);
return 1;
}
else
{
/* Failed */
if(task->command_inf.callback != NULL)
task->command_inf.callback(task, ioret, 0);
return 0;
}
}
int Delete(unsigned long *random_seed, param_t *params)
{
int errors = 0;
void *value;
long int_value;
int_value = get_random(random_seed) % params->scale + 1;
value = rb_remove(My_Tree, int_value);
if (value == NULL) errors++;
return errors;
}
/*
Function invoked when the file has been taken over for mapping.
NOTE: IO src belongs to fmap descriptor!
*/
INT32 fmap_takeover_callback(struct fsio_event_source *iosrc, INT32 ioret)
{
struct pm_task *task = NULL;
struct vmm_fmap_descriptor *fm = MEMREGNODE2FMAPDESC(rb_search(&vmm.fmap_descriptors, iosrc->id));
struct vmm_memory_region *mreg;
struct pm_thread *thr = NULL;
if(fm == NULL) return 0;
task = tsk_get(fm->io_finished.params[0]); // params[0] has the task id
if(task == NULL) return 0;
/* If file open succeded send a successful response. */
if(ioret == IO_RET_ERR)
{
/* Takeover failed, remove the memory region from it's trees */
ma_remove(&task->vmm_info.regions, &fm->regions->tsk_node);
rb_remove(&task->vmm_info.regions_id, &fm->regions->tsk_id_node);
kfree(fm);
kfree(mreg);
}
else
{
/* Get our file id for this file */
fm->iosrc.file_id = fm->gnode.value = task->io_finished.params[0];
/* Add the FMAP descriptor to the global list */
rb_insert(&vmm.fmap_descriptors, &fm->gnode, FALSE);
}
/* Activate task threads again. */
task->state = TSK_NORMAL;
thr = task->first_thread;
while(thr != NULL)
{
sch_deactivate(thr);
thr = thr->next_thread;
}
if(task->command_inf.callback != NULL)
task->command_inf.callback(task, ioret, 0);
return 1;
}
static inline void usart_tx_irq(usart_dev *dev)
{
/* Check USART Transmit Data Register Empty Interrupt */
if (USART_GetITStatus(dev->USARTx, USART_IT_TXE) != RESET)
{
if (!rb_is_empty(dev->txrb))
{
dev->USARTx->DR = rb_remove(dev->txrb);
dev->txbusy = 1;
}
else
{
/* Disable the USART Transmit Data Register Empty Interrupt */
USART_ITConfig(dev->USARTx, USART_IT_TXE, DISABLE);
dev->txbusy = 0;
}
}
}
void rb_remove_child(rbt *t, rbnode *n)
{
if(n->next != NULL)
{
if(n->prev != NULL)
n->prev->next = n->next;
n->next->prev = n->prev;
n->next = NULL;
n->prev = NULL;
}
else if(n->prev != NULL)
{
n->prev->next = n->next;
if(n->next != NULL)
n->next->prev = n->prev;
n->next = NULL;
n->prev = NULL;
}
else
{
rb_remove(t, n);
}
}
int main(int argc, char *argv[])
{
rb_tree_t rb_tree;
rb_node_t *node;
unsigned int key_pool[KEY_POOL_CAPS];
unsigned int key;
for (int i = 0; i < KEY_POOL_CAPS; ++i) {
add_key:
key = arc4random() % 3000;
for (int j = i - 1; j >= 0; j--) {
if (key == key_pool[j]) {
goto add_key;
}
}
key_pool[i] = key;
printf("Add %u into key pool.\n", key_pool[i]);
}
rb_tree_init(&rb_tree, data_compare, data_destroy);
redo:
for (int i = 0; i < KEY_POOL_CAPS; ++i) {
data_node_t *data = (data_node_t *)malloc(sizeof(*data));
data->key = key_pool[i];
if (rb_insert(&(data->rb_node), &rb_tree)) {
printf("Key %u already exist.\n", data->key);
} else {
printf("Add %u into tree.\n", data->key);
}
}
printf("INIT: ************\n");
printf("Min:%u\n",
rb_entry(rb_first(&rb_tree), data_node_t, rb_node)->key);
printf("Max:%u\n",
rb_entry(rb_last(&rb_tree), data_node_t, rb_node)->key);
printf("Trav with rb_next:\n");
node = rb_first(&rb_tree);
while (node) {
printf("%u, ",rb_entry(node, data_node_t, rb_node)->key);
node = rb_next(node);
}
printf("\n");
printf("Trav with rb_prev:\n");
node = rb_last(&rb_tree);
while (node) {
printf("%u, ",rb_entry(node, data_node_t, rb_node)->key);
node = rb_prev(node);
}
printf("\n");
printf("ERASE: ************\n");
data_node_t search_node;
for (int i = 0; i < 5; i++) {
unsigned int j = arc4random() % KEY_POOL_CAPS;
search_node.key = key_pool[j];
rb_node_t *to_remove = rb_find(&(search_node.rb_node), &rb_tree);
if (to_remove) {
printf("Remove %u from tree.\n", search_node.key);
rb_remove(to_remove, &rb_tree);
free(rb_entry(to_remove, data_node_t, rb_node));
} else {
printf("Key %u not in tree.\n", search_node.key);
}
}
printf("PRINT: ************\n");
printf("Min:%u\n",
rb_entry(rb_first(&rb_tree), data_node_t, rb_node)->key);
printf("Max:%u\n",
rb_entry(rb_last(&rb_tree), data_node_t, rb_node)->key);
printf("Trav with rb_next:\n");
node = rb_first(&rb_tree);
while (node) {
printf("%u, ",rb_entry(node, data_node_t, rb_node)->key);
node = rb_next(node);
}
printf("\n");
printf("Trav with rb_prev:\n");
node = rb_last(&rb_tree);
while (node) {
printf("%u, ",rb_entry(node, data_node_t, rb_node)->key);
node = rb_prev(node);
}
printf("\n");
rb_clear(&rb_tree);
printf("PRINT2: ************\n");
node = rb_first(&rb_tree);
if (node) {
printf("Min:%u\n",
rb_entry(node, data_node_t, rb_node)->key);
}
node = rb_last(&rb_tree);
if (node) {
printf("Max:%u\n",
rb_entry(node, data_node_t, rb_node)->key);
}
printf("Trav with rb_next:\n");
node = rb_first(&rb_tree);
while (node) {
printf("%u, ",rb_entry(node, data_node_t, rb_node)->key);
node = rb_next(node);
}
printf("\n");
printf("Trav with rb_prev:\n");
node = rb_last(&rb_tree);
while (node) {
printf("%u, ",rb_entry(node, data_node_t, rb_node)->key);
node = rb_prev(node);
}
printf("\n");
goto redo;
return 0;
}
int8_t usb_getc(void)
{
return rb_remove(rxfifo);
}
/*
Unload the specified lib.
*/
BOOL vmm_lib_unload(struct pm_task *task, struct vmm_memory_region *mreg)
{
struct vmm_slib_descriptor *lib = (struct vmm_slib_descriptor*)mreg->descriptor;
if(lib == NULL)
{
ma_remove(&task->vmm_info.regions, &mreg->tsk_node);
rb_remove(&task->vmm_info.regions_id, &mreg->tsk_id_node);
kfree(mreg);
pman_print_dbg("PMAN vmm_lib_unload: Lib is LD\n");
return TRUE; // this can only happen on the LD mapping
}
pman_print_dbg("PMAN vmm_lib_unload: Claim memory from task.\n");
// claim memory from the referencing task
vmm_claim_region(task, mreg);
ma_remove(&task->vmm_info.regions, &mreg->tsk_node);
rb_remove(&task->vmm_info.regions_id, &mreg->tsk_id_node);
// fix regions list on the descriptor
if(mreg->prev == NULL)
lib->regions = mreg->next;
else
mreg->prev->next = mreg->next;
if(mreg->next)
mreg->next->prev = mreg->prev;
kfree(mreg);
lib->references--;
pman_print_dbg("PMAN vmm_lib_unload: Lib references %i\n", lib->references);
if(!lib->references && lib->loaded)
{
struct pm_task *ltask = tsk_get(lib->task);
pman_print_dbg("PMAN vmm_lib_unload: Lib UNLOADING\n");
/* Close the file but dont wait for a callback */
struct stdfss_close msg_close;
msg_close.command = STDFSS_CLOSE;
msg_close.thr_id = -1;
msg_close.file_id = ltask->io_event_src.file_id;
msg_close.ret_port = 50; // this port is clearly not open
send_msg(ltask->io_event_src.fs_service, STDFSS_PORT, &msg_close);
// destroy the task
tsk_destroy(ltask); // this should also free the path!
// remove it from the loaded list
if(lib->prev == NULL)
{
vmm.slibs = lib->next;
vmm.slibs->prev = NULL;
}
else
{
lib->prev->next = lib->next;
}
if(lib->next != NULL)
lib->next->prev = lib->prev;
kfree(lib);
pman_print_dbg("PMAN vmm_lib_unload: Lib FREED\n");
}
return TRUE;
}