static void acm_received(uint32_t _tkn, int32_t _amt)
{
	int attempts;
	for(attempts = 0; attempts < 5; attempts++)
	{
		if(task_start(&acm_parse_task, &acm_parse, (void*)_amt))
			break;

		bufferPrintf("ACM: Worker already running, yielding...\n");

		task_yield();
	}
}

void cmd_multitouch_setup(int argc, char** argv)
{
	if(argc < 3)
	{
		bufferPrintf("%s <constructed fw> <constructed fw len>\r\n", argv[0]);
		return;
	}

	uint8_t* constructedFW = (uint8_t*) parseNumber(argv[1]);
	uint32_t constructedFWLen = parseNumber(argv[2]);

	multitouch_setup(constructedFW, constructedFWLen);
}

void cmd_audiohw_speaker_vol(int argc, char** argv)
{
	if(argc < 2)
	{
		bufferPrintf("%s <loudspeaker volume> [speaker volume] (between 0 and 100... 'speaker' is the one next to your ear)\r\n", argv[0]);
		return;
	}

	int vol = parseNumber(argv[1]);
	
	loudspeaker_vol(vol);

	bufferPrintf("Set loudspeaker volume to: %d\r\n", vol);

	if(argc > 2)
	{
		vol = parseNumber(argv[2]);
		speaker_vol(vol);

		bufferPrintf("Set speaker volume to: %d\r\n", vol);
	}
}

void cmd_time(int argc, char** argv) {
	int day;
	int month;
	int year;
	int hour;
	int minute;
	int second;
	int day_of_week;
	pmu_date(&year, &month, &day, &day_of_week, &hour, &minute, &second);
	bufferPrintf("Current time: %02d:%02d:%02d, %s %02d/%02d/%02d GMT\r\n", hour, minute, second, get_dayofweek_str(day_of_week), month, day, year);
	//bufferPrintf("Current time: %02d:%02d:%02d, %s %02d/%02d/20%02d\r\n", pmu_get_hours(), pmu_get_minutes(), pmu_get_seconds(), pmu_get_dayofweek_str(), pmu_get_month(), pmu_get_day(), pmu_get_year());
	//bufferPrintf("Current time: %llu\n", pmu_get_epoch());
}

void displayFileLSLine(HFSPlusCatalogFile* file, const char* name) {
	bufferPrintf("%06o ", file->permissions.fileMode);
	bufferPrintf("%3d ", file->permissions.ownerID);
	bufferPrintf("%3d ", file->permissions.groupID);
	bufferPrintf("%12Ld ", file->dataFork.logicalSize);
	bufferPrintf("                 ");
	bufferPrintf("%s\r\n", name);
}

error_t block_device_register(block_device_t *_bdev)
{
	EnterCriticalSection();
	LinkedList *prev = bdev_list.prev;
	_bdev->list_ptr.prev = prev;
	_bdev->list_ptr.next = &bdev_list;
	prev->next = &_bdev->list_ptr;
	bdev_list.prev = &_bdev->list_ptr;
	LeaveCriticalSection();

	bufferPrintf("bdev: Registered new device, 0x%p.\n", _bdev);

	return block_device_setup(_bdev);
}

static error_t cmd_wlan_prog_real(int argc, char** argv)
{
	if(argc < 3) {
		bufferPrintf("Usage: %s <address> <len>\r\n", argv[0]);
		return -1;
	}

	uint32_t address = parseNumber(argv[1]);
	uint32_t len = parseNumber(argv[2]);

	wlan_prog_real((void*) address, len);

	return 0;
}

void buffer_dump_memory(uint32_t start, int length) {
	uint32_t curPos = start;
	int x = 0;
	while(curPos < (start + length)) {
		if(x == 0) {
			bufferPrintf("0x%08x:", (unsigned int) curPos);
		}
		bufferPrintf(" %08x", (unsigned int) GET_REG(curPos));
		if(x == 1) {
			bufferPrintf(" ");
		}
		if(x == 3) {
			bufferPrintf("\r\n");
			x = 0;
		} else {
			x++;
		}

		curPos += 4;
	}

	bufferPrintf("\r\n");
}

error_t bufzone_finished_allocs(bufzone_t* _zone)
{
	uint8_t* buff;

	if (_zone->state != 1) {
		bufferPrintf("bufzone_finished_allocs: bad state\r\n");
		return EINVAL;
	}

	_zone->size = ROUND_UP(_zone->size, 64);
	buff = yaftl_alloc(_zone->size);

	if (!buff) {
		bufferPrintf("bufzone_finished_alloc: No buffer.\r\n");
		return EINVAL;
	}

	_zone->buffer = (uint32_t)buff;
	_zone->endOfBuffer = (uint32_t)(buff + _zone->size);
	_zone->state = 2;

	return SUCCESS;
}

static error_t cmd_install(int argc, char** argv)
{
    if((argc > 2 && argc < 4) || argc > 4)
    {
        bufferPrintf("Usage: %s <address> <len>\n", argv[0]);
        return EINVAL;
    }

    if(argc == 4)
    {
        uint32_t offset = parseNumber(argv[1]);
        uint32_t len = parseNumber(argv[2]);
        bufferPrintf("Installing OIB from 0x%08x:%d.\n", offset, len);
        images_install((void*)offset, len, fourcc("ibot"), fourcc("ibox"));
    }
    else
    {
        bufferPrintf("Starting Install/Upgrade...\r\n");
        images_install(&_start, (uint32_t)&OpenIBootEnd - (uint32_t)&_start, fourcc("ibot"), fourcc("ibox"));
    }

    return SUCCESS;
}

void OpenIBootConsole()
{
	init_modules();
	bufferPrintf(	"  ___                   _ ____              _   \r\n"
					" / _ \\ _ __   ___ _ __ (_) __ )  ___   ___ | |_ \r\n"
					"| | | | '_ \\ / _ \\ '_ \\| |  _ \\ / _ \\ / _ \\| __|\r\n"
					"| |_| | |_) |  __/ | | | | |_) | (_) | (_) | |_ \r\n"
					" \\___/| .__/ \\___|_| |_|_|____/ \\___/ \\___/ \\__|\r\n"
					"      |_|                                       \r\n"
					"\r\n"
					"version: %s\r\n", OPENIBOOT_VERSION_STR);

	DebugPrintf("                    DEBUG MODE\r\n");
}

void multitouch_on()
{
	if(!MultitouchOn)
	{
		bufferPrintf("multitouch: powering on\r\n");
		gpio_pin_output(MT_GPIO_POWER, 0);
		udelay(200000);
		gpio_pin_output(MT_GPIO_POWER, 1);

		udelay(15000);
		MultitouchOn = TRUE;
        fingerData=0;
	}
}

static error_t virtual_block_to_physical_block(vfl_vsvfl_device_t *_vfl, uint32_t _vBank, uint32_t _vBlock, uint32_t *_pBlock)
{
	uint32_t pCE, pPage;

	if(!_vfl->virtual_to_physical) {
		bufferPrintf("vsvfl: virtual_to_physical hasn't been initialized yet!\r\n");
		return EINVAL;
	}

	_vfl->virtual_to_physical(_vfl, _vBank, _vfl->geometry.pages_per_block * _vBlock, &pCE, &pPage);
	*_pBlock = pPage / _vfl->geometry.pages_per_block;

	return SUCCESS;
}

static void processCommand(char* command) {
	int argc;
	char** argv = tokenize(command, &argc);

	if(strcmp(argv[0], "sendfile") == 0) {
		if(argc >= 2) {
			// enter file mode
			EnterCriticalSection();
			if(dataRecvBuffer == commandRecvBuffer) {
				dataRecvBuffer = (uint8_t*) parseNumber(argv[1]);
			}
			LeaveCriticalSection();
			free(argv);
			return;
		}
	}

	if(strcmp(argv[0], "getfile") == 0) {
		if(argc >= 3) {
			// enter file mode
			EnterCriticalSection();
			if(sendFileBytesLeft == 0) {
				sendFilePtr = (uint8_t*) parseNumber(argv[1]);
				sendFileBytesLeft = parseNumber(argv[2]);
			}
			LeaveCriticalSection();
			free(argv);
			return;
		}
	}

	OPIBCommand* curCommand = CommandList;

	int success = FALSE;
	while(curCommand->name != NULL) {
		if(strcmp(argv[0], curCommand->name) == 0) {
			curCommand->routine(argc, argv);
			success = TRUE;
			break;
		}
		curCommand++;
	}

	if(!success) {
		bufferPrintf("unknown command: %s\r\n", command);
	}

	free(argv);
}

error_t task_sleep(int _ms)
{
	EnterCriticalSection();

	if(CurrentRunning == IRQTask)
	{
		LeaveCriticalSection();

		bufferPrintf("tasks: You can't suspend an ISR.\r\n");
		return EINVAL;
	}

	TaskDescriptor *next = CurrentRunning->taskList.next;
	if(next == CurrentRunning)
	{
		LeaveCriticalSection();
		
		bufferPrintf("tasks: Last thread cannot sleep!\n");

		uint64_t start = timer_get_system_microtime();
		while(!has_elapsed(start, _ms*1000));
		return SUCCESS;
	}

	uint32_t ticks = _ms * 1000;
	TaskDescriptor *task = CurrentRunning;

	//bufferPrintf("tasks: Putting task %p to sleep for %d ms.\n", task, _ms);
	task_remove(task);
	event_add(&task->sleepEvent, ticks, &task_wake_event, task);
	SwapTask(next);

	LeaveCriticalSection();

	return SUCCESS;
}

void cmd_ramdisk(int argc, char** argv) {
	uint32_t address;
	uint32_t size;

	if(argc < 3) {
		address = 0x09000000;
		size = received_file_size;
	} else {
		address = parseNumber(argv[1]);
		size = parseNumber(argv[2]);
	}

	set_ramdisk((void*) address, size);
	bufferPrintf("Loaded ramdisk at %08x - %08x\r\n", address, address + size);
}

static error_t physical_block_to_virtual_block(vfl_vsvfl_device_t *_vfl, uint32_t _pBlock, uint32_t *_vBank, uint32_t *_vBlock)
{
	uint32_t vBank, vPage;

	if(!_vfl->physical_to_virtual) {
		bufferPrintf("vsvfl: physical_to_virtual hasn't been initialized yet!\r\n");
		return EINVAL;
	}

	_vfl->physical_to_virtual(_vfl, 0, _vfl->geometry.pages_per_block * _pBlock, &vBank, &vPage);
	*_vBank = vBank / _vfl->geometry.num_ce;
	*_vBlock = vPage / _vfl->geometry.pages_per_block;

	return SUCCESS;
}

int radio_setup_3g()
{
	gpio_pulldown_configure(RADIO_BB_PULLDOWN, GPIOPDDown);

	pmu_gpio(RADIO_GPIO_BB_ON, TRUE, OFF);
	udelay(100000);
	gpio_pin_output(RADIO_GPIO_RADIO_ON, ON);
	udelay(100000);
	gpio_pin_output(RADIO_GPIO_BB_RESET, ON);
	udelay(100000);
	gpio_pin_output(RADIO_GPIO_BB_RESET, OFF);
	udelay(100000);

	gpio_pin_use_as_input(RADIO_GPIO_RESET_DETECT);
	if(gpio_pin_state(RADIO_GPIO_RESET_DETECT) != 1)
	{
		bufferPrintf("radio: comm board not present, powered on, or at+xdrv=10,2 had been issued.\r\n");
		return -1;
	}

	bufferPrintf("radio: comm board detected.\r\n");

	if(!radio_wait_for_ok(10))
	{
		bufferPrintf("radio: no response from baseband!\r\n");
		return -1;
	}

	RadioAvailable = TRUE;

	bufferPrintf("radio: ready.\r\n");

	speaker_setup();

	return 0;
}

int radio_setup_2g()
{
	gpio_pin_output(RADIO_GPIO_BB_MUX_SEL, OFF);

	gpio_pulldown_configure(RADIO_BB_PULLDOWN, GPIOPDDown);

	gpio_pin_output(RADIO_GPIO_BB_ON, OFF);
	udelay(100000);
	gpio_pin_output(RADIO_GPIO_RADIO_ON, ON);
	udelay(100000);
	gpio_pin_output(RADIO_GPIO_BB_RESET, ON);
	udelay(100000);
	gpio_pin_output(RADIO_GPIO_BB_RESET, OFF);
	udelay(100000);

	gpio_pin_use_as_input(RADIO_GPIO_BB_DETECT);
	if(gpio_pin_state(RADIO_GPIO_BB_DETECT) != 0)
	{
		bufferPrintf("radio: comm board not present, powered on, or at+xdrv=10,2 had been issued.\r\n");
		return -1;
	}

	bufferPrintf("radio: comm board detected.\r\n");

    
	if(!radio_wait_for_ok(10))
	{
		bufferPrintf("radio: no response from baseband!\r\n");
		return -1;
	}
  

	bufferPrintf("radio: setting speed to 750000 baud.\r\n");

	radio_write("at+ipr=750000\r\n");

	// wait a millisecond for the command to totally clear uart
	// I wasn't able to detect this condition with uart registers (looking at FIFO count, transmitter empty)
	udelay(1000);

	uart_set_baud_rate(RADIO_UART, 750000);

	if(!radio_wait_for_ok(10))
	{
		bufferPrintf("radio: no response from baseband!\r\n");
		return -1;
	}

	RadioAvailable = TRUE;

	bufferPrintf("radio: ready.\r\n");

	speaker_setup();

	return 0;
}

void cmd_audiohw_play_pcm(int argc, char** argv)
{
	if(argc < 3) {
		bufferPrintf("Usage: %s <address> <len> [use-headphones]\r\n", argv[0]);
		return;
	}

	uint32_t address = parseNumber(argv[1]);
	uint32_t len = parseNumber(argv[2]);
	uint32_t useHeadphones = 0;

	if(argc > 3)
		useHeadphones = parseNumber(argv[3]);

	if(useHeadphones)
	{
		bufferPrintf("playing PCM 0x%x - 0x%x using headphones\r\n", address, address + len);
		audiohw_play_pcm((void*)address, len, FALSE);
	} else
	{
		bufferPrintf("playing PCM 0x%x - 0x%x using speakers\r\n", address, address + len);
		audiohw_play_pcm((void*)address, len, TRUE);
	}
}