static struct liballoc_major *allocate_new_page(unsigned int size)
{
unsigned int st;
struct liballoc_major *maj;
// This is how much space is required.
st = size + sizeof(struct liballoc_major);
st += sizeof(struct liballoc_minor);
// Perfect amount of space?
if ((st % l_pageSize) == 0)
st = st / (l_pageSize);
else
st = st / (l_pageSize)+1;
// No, add the buffer.
// Make sure it's >= the minimum size.
if (st < l_pageCount) st = l_pageCount;
maj = (struct liballoc_major*)liballoc_alloc(st);
if (maj == NULL)
{
l_warningCount += 1;
#if defined DEBUG || defined INFO
write_serial_string("liballoc: WARNING: liballoc_alloc( %i ) return NULL\n");
FLUSH();
#endif
return NULL; // uh oh, we ran out of memory.
}
maj->prev = NULL;
maj->next = NULL;
maj->pages = st;
maj->size = st * l_pageSize;
maj->usage = sizeof(struct liballoc_major);
maj->first = NULL;
l_allocated += maj->size;
#ifdef DEBUG
write_serial_string("liballoc: Resource allocated %x of %i pages (%i bytes) for %i size.\n", maj, st, maj->size, size);
write_serial_string("liballoc: Total memory usage = %i KB\n", (int)((l_allocated / (1024))));
FLUSH();
#endif
return maj;
}
void klog(char *message) {
write_serial_string(COM1, message);
}
void initialize_klog() {
initialize_serial_port(COM1);
write_serial_string(COM1, "Logger initialized!\n");
}
int main(void)
{
setup();
char response_buffer[128];
bool resend = false;
GCODE_STATE previous_command; //stores the previous gcode command
GCODE_STATE current_command; //stores the next gcode command
init_gcode_state(&previous_command);
init_gcode_state(¤t_command);
KINEMATIC_STATE current;
KINEMATIC_STATE goal;
init_kinematic_state(¤t);
init_kinematic_state(&goal);
test_extuder(4000,4000);
calibrate(6000,3000);
while (1){
//should echo back any string sent to board over serial
usbd_poll(usbd_dev);
if(strlen(usb_buf) > 0){
//if it's a re-sent command we don't want to overwrite the previous
//command with something broken
if (resend == false){
previous_command = current_command;
}else{
//clear the resend
resend = false;
}
current_command = parse_gcode(usb_buf);
clear_USB_buffer();
current_command.x_offset = previous_command.x_offset;
current_command.y_offset = previous_command.y_offset;
current_command.z_offset = previous_command.z_offset;
current_command.extrude_offset = previous_command.extrude_offset;
//current_command.feed_rate = previous_command.feed_rate;
//Handle checksums
if(current_command.checksum_set){
if (current_command.checksum != current_command.calculated_checksum){
//if things get buggered, send a resend command
sprintf(response_buffer, "rs %i\n", current_command.line_number);
write_serial(response_buffer);
resend = true;
continue;
}
}
//Handle GCommands
if (current_command.g_command_set){
//G0 Rapid move
//G1 Controlled move
if (current_command.g_command == 0.0f || current_command.g_command == 1.0f){
if (current_command.x_set) goal.x = current_command.x + current_command.x_offset;
if (current_command.y_set) goal.y = current_command.y + current_command.y_offset;
//remember - z is inverted
if (current_command.z_set) goal.z = (-current_command.z) + current_command.z_offset;
if (current_command.extrude_length_set) goal.extrude_length = current_command.extrude_length + current_command.extrude_offset;
if(current_command.feed_rate_set){
goal.feed_rate = current_command.feed_rate;
}else{
//current_command.feed_rate = previous_command.feed_rate;
//goal.feed_rate = current_command.feed_rate;
}
current = line_generator(current, goal, step_motors);
}
//G92 Set Position (without move)
if (current_command.g_command == 92.0f){
if (current_command.x_set){
current_command.x_offset = current.x - current_command.x;
}
if (current_command.y_set){
current_command.y_offset = current.y - current_command.y;
}
if (current_command.z_set){
//remember - z is inverted
current_command.z_offset = current.z - (-current_command.z);
}
if (current_command.extrude_length_set){
current_command.extrude_offset = current.extrude_length - current_command.extrude_length;
}
}
}
if (current_command.m_command_set){
//TODO: Sort the spindle code out so that it can be rotated in both directions
//for now it is only used to control an acuator which is off or on.
//M3 Spindle ON clockwise
if (current_command.m_command == 3.0f){
set_spindle(true);
}
//M4 Spindle ON counter-clockwise
if (current_command.m_command == 3.0f){
set_spindle(true);
}
//M5 Spindle OFF
if (current_command.m_command == 5.0f){
set_spindle(false);
}
//M105 get extruder temperature
if (current_command.m_command == 105.0f){
sprintf(response_buffer, "ok\n T:%d", read_adc());
write_serial_string(response_buffer);
continue;
}
}
//DO NOT PUT CODE BELOW THIS POINT UNLESS THE "continue"'s in the above code
//are taken care of or not important.
//use the repsponse buffer because fuckit
sprintf(response_buffer, "ok\n");
write_serial_string(response_buffer);
}
}
}
void *PREFIX(malloc)(size_t req_size)
{
int startedBet = 0;
unsigned long long bestSize = 0;
void *p = NULL;
uintptr_t diff;
struct liballoc_major *maj;
struct liballoc_minor *min;
struct liballoc_minor *new_min;
unsigned long size = req_size;
// For alignment, we adjust size so there's enough space to align.
if (ALIGNMENT > 1)
{
size += ALIGNMENT + ALIGN_INFO;
}
// So, ideally, we really want an alignment of 0 or 1 in order
// to save space.
liballoc_lock();
if (size == 0)
{
l_warningCount += 1;
#if defined DEBUG || defined INFO
write_serial_string("liballoc: WARNING: alloc( 0 ) called from %x\n");
FLUSH();
#endif
liballoc_unlock();
return PREFIX(malloc)(1);
}
if (l_memRoot == NULL)
{
#if defined DEBUG || defined INFO
#ifdef DEBUG
kprintf("liballoc: initialization of liballoc " VERSION "\n");
#endif
FLUSH();
#endif
// This is the first time we are being used.
l_memRoot = allocate_new_page(size);
if (l_memRoot == NULL)
{
liballoc_unlock();
#ifdef DEBUG
write_serial_string("liballoc: initial l_memRoot at %p initialization failed\n", p);
FLUSH();
#endif
return NULL;
}
#ifdef DEBUG
write_serial_string("liballoc: set up first memory major %x\n", l_memRoot);
FLUSH();
#endif
}
#ifdef DEBUG
write_serial_string("liballoc: %x PREFIX(malloc)( %i ): ",
__builtin_return_address(0),
size);
FLUSH();
#endif
// Now we need to bounce through every major and find enough space....
maj = l_memRoot;
startedBet = 0;
// Start at the best bet....
if (l_bestBet != NULL)
{
bestSize = l_bestBet->size - l_bestBet->usage;
if (bestSize > (size + sizeof(struct liballoc_minor)))
{
maj = l_bestBet;
startedBet = 1;
}
}
while (maj != NULL)
{
diff = maj->size - maj->usage;
// free memory in the block
if (bestSize < diff)
{
// Hmm.. this one has more memory then our bestBet. Remember!
l_bestBet = maj;
bestSize = diff;
}
#ifdef USE_CASE1
// CASE 1: There is not enough space in this major block.
if (diff < (size + sizeof(struct liballoc_minor)))
{
#ifdef DEBUG
kprintf("CASE 1: Insufficient space in block %x\n", maj);
FLUSH();
#endif
// Another major block next to this one?
if (maj->next != NULL)
{
maj = maj->next; // Hop to that one.
continue;
}
if (startedBet == 1) // If we started at the best bet,
{ // let's start all over again.
maj = l_memRoot;
startedBet = 0;
continue;
}
// Create a new major block next to this one and...
maj->next = allocate_new_page(size); // next one will be okay.
if (maj->next == NULL) break; // no more memory.
maj->next->prev = maj;
maj = maj->next;
// .. fall through to CASE 2 ..
}
#endif
#ifdef USE_CASE2
// CASE 2: It's a brand new block.
if (maj->first == NULL)
{
maj->first = (struct liballoc_minor*)((uintptr_t)maj + sizeof(struct liballoc_major));
maj->first->magic = LIBALLOC_MAGIC;
maj->first->prev = NULL;
maj->first->next = NULL;
maj->first->block = maj;
maj->first->size = size;
maj->first->req_size = req_size;
maj->usage += size + sizeof(struct liballoc_minor);
l_inuse += size;
l_max_inuse = (l_inuse>l_max_inuse) ? l_inuse : l_max_inuse;
p = (void*)((uintptr_t)(maj->first) + sizeof(struct liballoc_minor));
ALIGN(p);
#ifdef DEBUG
kprintf("CASE 2: returning %x\n", p);
FLUSH();
#endif
liballoc_unlock(); // release the lock
return p;
}
#endif
#ifdef USE_CASE3
// CASE 3: Block in use and enough space at the start of the block.
diff = (uintptr_t)(maj->first);
diff -= (uintptr_t)maj;
diff -= sizeof(struct liballoc_major);
if (diff >= (size + sizeof(struct liballoc_minor)))
{
// Yes, space in front. Squeeze in.
maj->first->prev = (struct liballoc_minor*)((uintptr_t)maj + sizeof(struct liballoc_major));
maj->first->prev->next = maj->first;
maj->first = maj->first->prev;
maj->first->magic = LIBALLOC_MAGIC;
maj->first->prev = NULL;
maj->first->block = maj;
maj->first->size = size;
maj->first->req_size = req_size;
maj->usage += size + sizeof(struct liballoc_minor);
l_inuse += size;
l_max_inuse = (l_inuse>l_max_inuse) ? l_inuse : l_max_inuse;
p = (void*)((uintptr_t)(maj->first) + sizeof(struct liballoc_minor));
ALIGN(p);
#ifdef DEBUG
kprintf("CASE 3: returning %x\n", p);
FLUSH();
#endif
liballoc_unlock(); // release the lock
return p;
}
#endif
#ifdef USE_CASE4
// CASE 4: There is enough space in this block. But is it contiguous?
min = maj->first;
// Looping within the block now...
while (min != NULL)
{
// CASE 4.1: End of minors in a block. Space from last and end?
if (min->next == NULL)
{
// the rest of this block is free... is it big enough?
diff = (uintptr_t)(maj)+maj->size;
diff -= (uintptr_t)min;
diff -= sizeof(struct liballoc_minor);
diff -= min->size;
// minus already existing usage..
if (diff >= (size + sizeof(struct liballoc_minor)))
{
// yay....
min->next = (struct liballoc_minor*)((uintptr_t)min + sizeof(struct liballoc_minor) + min->size);
min->next->prev = min;
min = min->next;
min->next = NULL;
min->magic = LIBALLOC_MAGIC;
min->block = maj;
min->size = size;
min->req_size = req_size;
maj->usage += size + sizeof(struct liballoc_minor);
l_inuse += size;
l_max_inuse = (l_inuse>l_max_inuse) ? l_inuse : l_max_inuse;
p = (void*)((uintptr_t)min + sizeof(struct liballoc_minor));
ALIGN(p);
#ifdef DEBUG
kprintf("CASE 4.1: returning %x\n", p);
FLUSH();
#endif
liballoc_unlock(); // release the lock
return p;
}
}
// CASE 4.2: Is there space between two minors?
if (min->next != NULL)
{
// is the difference between here and next big enough?
diff = (uintptr_t)(min->next);
diff -= (uintptr_t)min;
diff -= sizeof(struct liballoc_minor);
diff -= min->size;
// minus our existing usage.
if (diff >= (size + sizeof(struct liballoc_minor)))
{
// yay......
new_min = (struct liballoc_minor*)((uintptr_t)min + sizeof(struct liballoc_minor) + min->size);
new_min->magic = LIBALLOC_MAGIC;
new_min->next = min->next;
new_min->prev = min;
new_min->size = size;
new_min->req_size = req_size;
new_min->block = maj;
min->next->prev = new_min;
min->next = new_min;
maj->usage += size + sizeof(struct liballoc_minor);
l_inuse += size;
p = (void*)((uintptr_t)new_min + sizeof(struct liballoc_minor));
ALIGN(p);
#ifdef DEBUG
kprintf("CASE 4.2: returning %x\n", p);
FLUSH();
#endif
liballoc_unlock(); // release the lock
return p;
}
} // min->next != NULL
min = min->next;
} // while min != NULL ...
#endif
#ifdef USE_CASE5
// CASE 5: Block full! Ensure next block and loop.
if (maj->next == NULL)
{
#ifdef DEBUG
kprintf("CASE 5: block full\n");
FLUSH();
#endif
if (startedBet == 1)
{
maj = l_memRoot;
startedBet = 0;
continue;
}
// we've run out. we need more...
maj->next = allocate_new_page(size); // next one guaranteed to be okay
if (maj->next == NULL) break; // uh oh, no more memory.....
maj->next->prev = maj;
}
#endif
maj = maj->next;
} // while (maj != NULL)
liballoc_unlock(); // release the lock
#ifdef DEBUG
write_serial_string("All cases exhausted. No memory available.\n");
FLUSH();
#endif
#if defined DEBUG || defined INFO
FLUSH();
#endif
return NULL;
}
