/*
 * tt-loader - A system loader through USB derived from OMAP Flash Loader
 *
 * Copyright (C) 2008 Guillaume Bougard <gbougard@pkg.fr>
 * Copyright (C) 2005 Luis Recuerda <lrec@helios.homeip.net>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * as published by the Free Software Foundation; either version 2
 * of the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
 *
 */

#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/times.h>
#include <fcntl.h>
#include <usb.h>
#include <endian.h>
#include <byteswap.h>
#include <errno.h>

extern unsigned long crc32( unsigned long crc, char* buf, unsigned int len);

/*
 * Here Vendor/Product detected for the target device
 */
#define	OMAP_VENDOR		0x0451
#define	OMAP_PRODUCT	0x3f01

#define	IN_EP	0x81
#define	OUT_EP	0x02

/*
 * Length of memory dump lines
 */
#define DUMP_LINE_SIZE		32

struct mem_file {
	void *content ;
	int size ;
	char *filename ;
};

/*
 * MAX_SIZE is the maximum size of the 2nd.bin program
 */
#define	MAX_SIZE	65536
#define CHUNK_SIZE	8192
static char	buffer[MAX_SIZE + 128];
static int	buffsize = 0;
static struct mem_file cmdfile = { NULL, 0, NULL };
static double btime ;
static double ticks ;

#define log1(X)		fprintf(stderr, "%9.3f: "X,((double)times(NULL)-btime)/ticks)
#define log2(X,Y) 	fprintf(stderr, "%9.3f: "X,((double)times(NULL)-btime)/ticks,(Y))
#define log3(X,Y,Z)	fprintf(stderr, "%9.3f: "X,((double)times(NULL)-btime)/ticks,(Y),(Z))
#define log4(W,X,Y,Z)	fprintf(stderr, "%9.3f: "W,((double)times(NULL)-btime)/ticks,(X),(Y),(Z))

#if __BYTE_ORDER == __LITTLE_ENDIAN
# define cpu_to_le32(x)	(x)
# define le32_to_cpu(x)	(x)
#else
# define cpu_to_le32(x)	bswap_32 (x)
# define le32_to_cpu(x)	bswap_32 (x)
#endif

static inline unsigned round_up (unsigned v, unsigned d)
{
	v+= --d;
	return v & ~d;
}

static int stringcopy( char * dest, const char *src, int max )
{
	char *tmp = dest, *s = (char *) src;
	int count = 0 ;
	
	while (count++ < max)
		if (!(*tmp++ = *s++ ))
			break ;
	
	return count;
}

static int send_cmd (usb_dev_handle *handle, const char req, const char *src)
{
	int	res, len = 5 ;
	
	buffer[0]= 'T';
	buffer[1]= 'I';
	buffer[2]= 'S';
	buffer[3]= req ;
	buffer[4]= '\0' ;
	if (src!=NULL)
		len += stringcopy (&buffer[4], src, 60);
	else
		len = 4 ;
	buffer[63]= '\0' ; // truncate anyway
	
	res= usb_bulk_write (handle, OUT_EP, buffer, len, 1000);
	if (res < len)
	{
		log2("Error in usb_bulk_write during send_char: %d/4\n", res);
		return 0;
	}
	
	return 1;
}

static int send_binsize (usb_dev_handle *handle, int size)
{
	int	res;
	buffer[0]= 'T';
	buffer[1]= 'I';
	buffer[2]= 'S';
	buffer[3]= 's';
	*(u_int32_t *) &buffer[4]= cpu_to_le32 ( size < CHUNK_SIZE ? size : CHUNK_SIZE );
	
	res= usb_bulk_write (handle, OUT_EP, buffer, 8, 1000);
	if (res < 8)
	{
		log2("Error in usb_bulk_write during send_binsize: %d/8\n", res);
		return 0;
	}
	
	return 1;
}

static int send_word (usb_dev_handle *handle, const char req, int word)
{
	int	res;
	buffer[0]= 'T';
	buffer[1]= 'I';
	buffer[2]= 'S';
	buffer[3]= req;
	*(u_int32_t *) &buffer[4]= cpu_to_le32 (word);
	
	res= usb_bulk_write (handle, OUT_EP, buffer, 8, 1000);
	if (res < 8)
	{
		log2("Error in usb_bulk_write: %d/8\n", res);
		return 0;
	}
	
	return 1;
}

static int send_poke (usb_dev_handle *handle, int poke)
{
	int	res;
	buffer[0]= 'T';
	buffer[1]= 'I';
	buffer[2]= 'S';
	buffer[3]= 'p';
	*(u_int32_t *) &buffer[4]= cpu_to_le32 (poke);
	
	res= usb_bulk_write (handle, OUT_EP, buffer, 8, 1000);
	if (res < 8)
	{
		log2("Error in usb_bulk_write: %d/8\n", res);
		return 0;
	}
	
	return 1;
}

static struct mem_file readfile( char *filename )
{
	struct mem_file toread = { NULL, 0, NULL };
	int fd= open (filename, O_RDONLY);
	
	if (fd < 0)
	{
		log3("Error opening %s file (err=%d)\n", filename, errno);
		return toread;
	}
	
	toread.size = lseek (fd, 0, SEEK_END);
	if (toread.size < 0  ||  lseek (fd, 0, SEEK_SET) < 0)
	{
		log3("Error with lseek other %s file (err=%d)\n", filename, errno);
		close (fd);
		return toread;
	}
	
	toread.content= malloc (round_up (toread.size, 4));
	if (toread.content == NULL)
	{
		log2("Out of memory requesting %d bytes\n", toread.size);
		close (fd);
		return toread;
	}
	
	if ((toread.size= read (fd, toread.content, toread.size)) < 0)
	{
		log3("Error reading %s file (err=%d)\n", filename, errno);
		close (fd);
		return toread;
	}
	
	close (fd);
	
	return toread ;
}

static int savefile( char *filename, struct mem_file buffer )
{
	int fd= creat (filename, S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP|S_IROTH );
	
	if (fd < 0)
	{
		log3("Error opening %s file for writing (err=%d)\n", filename, errno);
		return 1;
	}
	
	if (buffer.size != write (fd, buffer.content, buffer.size))
	{
		log3("Error writing %s file (err=%d)\n", filename, errno);
		close (fd);
		return 1;
	}
	
	close (fd);
	
	return 0 ;
}

static int process (usb_dev_handle *handle)
{
	int	err = -1 ;
	
	log1("OMAP found, trying to configure it\n");
	
	for (;;)
	{
		err= usb_set_configuration (handle, 1);
		if (err == 0)
			break;
		
		if (err == -ENODEV)
		{
			log1("OMAP error, retrying\n");
			return 1;
		}
		
		sleep (1);
	}
	
	err = usb_claim_interface (handle, 0);
	if ( err < 0)
		log2("Error in usb_claim_interface (%d)\n",err);
	else
	{
		char inbuff[MAX_SIZE + 128];
		int	insize= usb_bulk_read (handle, IN_EP, inbuff, sizeof (inbuff), 1000);
		
		if (insize < 48)
			log2("Error in usb_bulk_read: %d\n", insize);
		else
		{
			log1("OMAP 1st boot contacted, sending 2nd boot\n");
			
			memcpy (&buffer[20], inbuff, 44);
			memcpy (&buffer[80], inbuff, 48);
			
			char *p	= buffer;
			int	size= buffsize;
			while (size > 0)
			{
				int chunksize = size < CHUNK_SIZE ? size : CHUNK_SIZE ;
				int	outsize = usb_bulk_write (handle, OUT_EP, p, chunksize, 1000);
				if (outsize < chunksize)
				{
					log3("Error in usb_bulk_write: %d/%d\n", outsize,chunksize);
					break;
				}

				p+= chunksize;
				size-= chunksize;
			}
			
			if (size <= 0)
			{
				log2("Sent %d bytes to OMAP\n",buffsize);
				
				p= NULL;
				size= 0;
				
				int	res=1, sz= 0, loop= 1, cmd=0, ftag=0, finfo=0, fline=0 ;
				int vendor, device, info ;
				char *cmdp = cmdfile.content ;
				struct mem_file current = { NULL, 0, NULL };
				
				err = 1 ; // Set error by default
				
				while (loop != 0)
				{
					if (loop<0) loop++ ; // Avoid infinite loop
					
					if (res>0 || ftag)
						res= usb_bulk_read (handle, IN_EP,
							inbuff, sizeof (inbuff), 5000);
					
					if (res < 0)
					{
						log2("Error in usb_bulk_read: %d\n",res);
						break;
					}
					
					if (ftag)
					{
						switch (ftag)
						{
							case 0xaaaa0001:
								// A string should be in the buffer with length still set in finfo
								if ( res != finfo)
								{
									log3("Error waiting string from flasher program: length = %d vs %d expected\n", res, finfo);
									break ;
								}
								// Fix the string just in case
								inbuff[res] = '\0' ;
								if (fline)
									fprintf(stderr, "%s",inbuff);
								else
									log2("Flasher: %s", inbuff);
								if ( inbuff[res-1] == '\n' )
									fline = 0 ;
								else
									fline ++ ;
								break ;
							case 0xaaaa0002:
								finfo = le32_to_cpu (*(u_int32_t *) inbuff);
								break ;
							case 0xaaaa0003:
								finfo = le32_to_cpu (*(u_int32_t *) inbuff);
								log2("Flasher: NAND read = 0x%08X\n", finfo);
								break ;
							case 0xaaaa0004:
								finfo = le32_to_cpu (*(u_int32_t *) inbuff);
								log2("Flasher: NAND size = 0x%08X\n", finfo);
								break ;
							case 0xaaaa0005:
								finfo = le32_to_cpu (*(u_int32_t *) inbuff);
								log2("Flasher: Error code = %d\n", finfo);
								err = - finfo ; // Will quit on error
								break ;
							case 0xaaaa0006:
								finfo = le32_to_cpu (*(u_int32_t *) inbuff);
								log2("Flasher: NAND remain = 0x%08X\n", finfo);
								break ;
						}
						ftag = 0 ;
						continue ;
					}
					
					if (err<0)
					{
						err = -err ; // Keep err as positive number
						if (send_cmd (handle,'e',NULL))
							log1("Asking stop\n");
						else
							log1("Can't ask to stop\n");
						break ; // Leave loop
					}

					if (res >= 4)
					{
						if (inbuff[0] == 'T'  &&  inbuff[1] == 'I'  &&
							inbuff[2] == 'S')
						{
							switch (inbuff[3])
							{
							case 'f':
								// Target claims a file
								log3("OMAP File asked: %s (%d bytes)\n", &inbuff[4], size);
								if (!send_binsize (handle, size))
									loop = 0 ; // Just quit on error
								break;
								
							case 'o':
								// Update upload file buffer offset
								sz= le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								p+= sz;
								size-= sz;
								//log3("OMAP seek %d to 0x%08lX\n", sz, (long)p-(long)current.content); 					// DEBUG
								if (size > 0  &&  !send_binsize (handle, size))
									loop = 0 ; // Just quit on error
								if (size==0)
									res = 0 ;
								break;
								
							case 'n':
								// Upload a requested size chunk from the file buffer
								sz= le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								//log2("OMAP read %d\n", sz); 					// DEBUG
								res= usb_bulk_write (handle, OUT_EP, p, sz, 1000);
								if (res < sz)
								{
									log3("Error in usb_bulk_write: %d/%d\n", res, sz);
									loop = 0 ; // Just quit on error
								}
								break;
								
							case 'I':
								info = le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								log2("OMAP integer info: %d\n", info);
								break;
								
							case 'x':
								info = le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								u_int32_t len = le32_to_cpu (*(u_int32_t *) &inbuff[8]);
								u_int32_t crc = le32_to_cpu (*(u_int32_t *) &inbuff[12]);
								log4("OMAP crc32 computing: pos 0x%08x/0x%08x, current crc32 0x%08X\n",info,len,crc);
								break;
								
							case 'A':
								info = le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								log2("OMAP address info: 0x%08X\n", info);
								break;
								
							case 'C':
								info = le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								log2("OMAP crc32 found: 0x%08X\n", info);
								break;
								
							case 'i':
								info = le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								log2("OMAP Info returned: 0x%08X\n", info);
								res = 0 ; // Target is waiting a new command
								break;
								
							case 'v':
								vendor= le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								log2("Flash Vendor found: 0x%02X\n", vendor);
								break;
								
							case 'd':
								device= le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								log2("Flash Device found: 0x%02X\n", device);
								break;
								
							case 'm':
								log2("OMAP Message: %s\n", &inbuff[4]);
								break;
								
							case 'D':
								if (!current.content)
								{
									log1("Can't dump memory without buffer\n");
									loop = 0 ;
									break ;
								}
								res -= 4 ;
								if (res < 1)
								{
									log2("Received empty memory dump (res=%d)\n",res);
									// reset buffer size anyway
									current.size = 0 ;
									res = 0 ; // Target is waiting a new command
									
								} else {
									if (res > size)
									{
										log2("Can't dump %d bytes as buffer is full\n",res);
										loop = 0 ;
										break ;
									}
									memcpy(p,&inbuff[4],res);
									p += res ;
									size -= res ;
								}
								if ( size == 0 )
								{
									log2("%d bytes dumped to buffer\n",current.size);
									res = 0 ; // Target is waiting a new command
								}
								break;
								
							case 'M':
								sz = 0 ;
								res -= 4 ;
								while (sz<res)
								{
									log2("0x%08X | ",info);
									
									loop=0 ;
									while (++loop<=DUMP_LINE_SIZE)
										fprintf (stderr,"%02hhX", inbuff[3+sz+loop]);
									fprintf (stderr, " |");
									
									loop=0 ;
									while (++loop<=DUMP_LINE_SIZE)
										if ((int)inbuff[3+sz+loop]>=0x20 && (int)inbuff[3+sz+loop]<0x7f)
											fprintf (stderr,"%c", inbuff[3+sz+loop]);
										else
											fprintf (stderr,".");
									
									fprintf (stderr,"|\n");
									
									info += DUMP_LINE_SIZE;
									sz += DUMP_LINE_SIZE;
								}
								res = 0 ; // Target is waiting a new command
								break;
								
							case 'r':
								log2("OMAP is ready: %s\n", &inbuff[4]);
								res = 0 ; // Target is waiting a new command
								break;

							case 'b':
								// Boot command has been received
								info = le32_to_cpu (*(u_int32_t *) &inbuff[4]);
								log2("OMAP is booting at address: 0x%08X\n", info);
								loop = err = res = 0 ; // Quit now
								break;

							default:
								log2("Unknown packet type '%c'\n", inbuff[3]);
								break;
							}
						} else
						{
							info = le32_to_cpu (*(u_int32_t *) inbuff);
							switch (info)
							{
								case 0xaaaa0001:
								case 0xaaaa0002:
								case 0xaaaa0003:
								case 0xaaaa0004:
								case 0xaaaa0005:
								case 0xaaaa0006:
									ftag = info ;
									break ;
								case 0xff555580:
									log1("Flasher: Command finished\n");
									break ;
								default:
									// Fix the string just in case
									inbuff[res] = '\0' ;
									// Strip final EOL in the string
									if ( inbuff[res-1] == '\n' )
										inbuff[res-1] = '\0' ;
									if (res==4)
										log3("Got: %s (0x%08X)\n", inbuff, info);
									else
										log2("Got: %s\n", inbuff);
							}
						}
					}
					else
					{
						// Manage commands
						if (cmd + 1 >= cmdfile.size)
						{
							if (loop>0) {
								log1("Command file read finished\n");
								loop = 0 ;
							}
							
						} else {
							// res < 4
							//log2("Next command is '%c'\n", cmdp[cmd]); 			// DEBUG
							switch (cmdp[cmd])
							{
							case 'D': // Save downloaded memory to a file still specified
								if (current.filename == NULL)
								{
									log1("Can't save without a file specified with 'F' command\n");
									loop = 0 ; // Just quit on error
									break ;
								}
								if (current.content == NULL)
								{
									log2("Can't save without download buffer to file '%s'\n",current.filename);
									loop = 0 ; // Just quit on error
									break ;
								}
								if (current.size==0)
									log2("Nothing to save, file %s is still up to date\n",current.filename);
								else if (savefile(current.filename, current)==0)
								{
									info = crc32(0,current.content,current.size);
									log3("Downloaded memory saved to file %s with CRC32=0x%08X\n",current.filename,info);
								} else
								{
									log2("Can't save downloaded memory saved to file %s\n",current.filename);
									// Just quit on error
									loop = 0 ;
								}
								// Release memory
								free((void *)current.content);
								free((void *)current.filename);
								current.content = NULL ;
								current.filename = NULL ;
								res -- ;
								break;
							case 'd': // Save downloaded memory to a file
								cmd += 2 ;
								if (cmd >= cmdfile.size || cmdp[cmd] == '\0')
								{
									log1("Bad 'd' format in command download\n");
									break ;
								}
								if (current.content == NULL)
								{
									log2("Can't save without download buffer to file '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								if (current.size==0)
									log2("Nothing to save, file %s is still up to date\n",&cmdp[cmd]);
								else if (savefile(&cmdp[cmd], current)==0)
								{
									info = crc32(0,current.content,current.size);
									log3("Downloaded memory saved to file %s with CRC32=0x%08X\n",&cmdp[cmd],info);
								} else
								{
									log2("Can't save downloaded memory saved to file %s\n",&cmdp[cmd]);
									// Just quit on error
									loop = 0 ;
								}
								// Release memory
								free((void *)current.content);
								current.content = NULL ;
								res -- ;
								break;
							case 'f': // Put a file to be uploaded in a memory buffer
								cmd += 2 ;
								if (cmd >= cmdfile.size || cmdp[cmd] == '\0')
								{
									log1("Bad 'f' format in command file\n");
									break ;
								}
								// Free previously used memory buffer
								if (current.content != NULL)
									free((void *)current.content);
								current = readfile(&cmdp[cmd]);
								if (current.content == NULL)
								{
									log2("Can't read file '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								p = current.content ;
								size = current.size ;
								// Say to OMAP target it can reclaim the file
								if (send_cmd (handle,'f',(const char *)&cmdp[cmd]))
									log1("File loaded and ready for upload\n");
								else
								{
									log1("Can't say we are ready for upload\n");
									// Just quit on error
									loop = 0 ;
								}
								break;
							case 'F': // Set a filename that would be loaded to check crc and saved
								cmd += 2 ;
								if (cmd >= cmdfile.size || cmdp[cmd] == '\0')
								{
									log1("Bad 'F' format in command file\n");
									break ;
								}
								// Free previously used memory buffer
								if (current.filename != NULL)
									free((void *)current.filename);
								info = strlen(&cmdp[cmd]);
								if (info == 0)
								{
									log2("Can't read filename '%s' length\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								current.filename = malloc (++info);
								if (current.filename == NULL)
								{
									log2("Out of memory requesting %d bytes\n", info);
									loop = 0 ; // Just quit on error
									break ;
								}
								if (info!=stringcopy(current.filename,&cmdp[cmd],info))
								{
									log2("Can't read filename '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								// still read next command
								res -- ;
								break;
							case 'C': // Get a CRC32 and send it to target
								cmd += 2 ;
								if (sscanf((const char *)&cmdp[cmd], "%i", &info) != 1)
								{
									log2("Can't read crc32 '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								// compute the current CRC32 if a file is referenced and exists
								if (!info && current.filename != NULL)
								{
									struct mem_file loader = readfile(current.filename);
									if (loader.content == NULL)
									{
										log2("Can't read file '%s', will be initialized with target\n",current.filename);
									} else
									{
										log3("Checking CRC32 of %d bytes from %s file\n",loader.size,current.filename);
										info = crc32(0,loader.content,loader.size);
										free(loader.content);
									}
								}
								if (send_word (handle, 'C', info))
									log2("Sending crc32 0x%02X\n", info);
								else
								{
									log2("Can't send crc32 0x%02X\n", info);
									loop = 0 ; // Just quit on error
								}
								break;
							case 'a': // Get an address and send it to target
								cmd += 2 ;
								if (sscanf((const char *)&cmdp[cmd], "%i", &info) != 1)
								{
									log2("Can't read address '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								if (send_word (handle, 'a', info))
									log2("Sending address 0x%02X\n", info);
								else
								{
									log2("Can't send address 0x%02X\n", info);
									loop = 0 ; // Just quit on error
								}
								break;
							case 'm': // Get a size in kB and ask a memory download
								cmd += 2 ;
								if (sscanf((const char *)&cmdp[cmd], "%i", &info) != 1)
								{
									log2("Can't read size '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								// size unit is kbytes
								size = info * 1024 ;
								// Free previously used memory buffer
								if (current.content != NULL)
									free((void *)current.content);
								current.size = size ;
								p = malloc (round_up (size, 4));
								if ( p == NULL)
								{
									log2("Out of memory requesting %d bytes\n", round_up (size, 4));
									loop=0 ;
									break ;
								}
								current.content = p ;
								if (!(send_word (handle,'m',size)))
								{
									log2("Can't ask to download %d bytes from memory\n",size);
									loop = 0 ; // Just quit on error
								} else
									log2("Allocated %d kBytes to download memory\n",info);
								break;
							case 'M': // Ask memory dump
								if (!(send_cmd (handle,'M',NULL)))
								{
									log1("Can't ask to read memory\n");
									loop = 0 ; // Just quit on error
								}
								break;
							case 'P': // Ask a peek onto the address
								if (!(send_cmd (handle,'P',NULL)))
								{
									log1("Can't ask to peek memory\n");
									loop = 0 ; // Just quit on error
								}
								break;
							case 'S': // Ask to change stack to the last given address
								if (send_cmd (handle,'S',NULL))
									log1("Asking stack relocate\n");
								else
								{
									log1("Can't ask to relocate stack\n");
									loop = 0 ; // Just quit on error
								}
								break;
							case 'c': // Make a call to the last given address
								if (send_cmd (handle,'c',NULL))
									log1("Asking function call\n");
								else
								{
									log1("Can't ask to call a function\n");
									loop = 0 ; // Just quit on error
								}
								break;
							case 'b': // Boot the target by just branching to the last given address
								if (send_cmd (handle,'b',NULL)) {
									log1("Asking boot\n");
									// Quit now witout error set
									loop = err = -10 ;
									cmd = cmdfile.size ;
									res = 1 ;
								} else
								{
									log1("Can't ask to boot\n");
									loop = 0 ; // Just quit on error
								}
								break;
							case 'e': // End of commands
								log1("Commands read and sent\n");
								if (send_cmd (handle,'e',NULL))
									log1("Asking stop\n");
								else
									log1("Can't ask to stop\n");
								// Quit now witout error set
								loop = err = 0 ;
								cmd = cmdfile.size ;
								res = 1 ;
								break ;
							case 'k': // Boot a linux kernel to the last given address
								cmd += 2 ;
								if (sscanf((const char *)&cmdp[cmd], "%i", &info) != 1)
								{
									log2("Can't read mach id '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								if (send_word (handle,'k', info)) {
									log1("Asking kernel to boot\n");
									// Quit now witout error set
									loop = err = -10 ;
									cmd = cmdfile.size ;
									res = 1 ;
								} else
								{
									log1("Can't ask to boot a kernel\n");
									loop = 0 ; // Just quit on error
								}
								break;
							case 'p': // poke
								cmd += 2 ;
								if (sscanf((const char *)&cmdp[cmd], "%i", &info) != 1)
								{
									log2("Can't read value to poke '%s'\n",&cmdp[cmd]);
									loop = 0 ; // Just quit on error
									break ;
								}
								if (send_poke (handle, info))
									log2("Sending poke 0x%08X\n", info);
								else
								{
									log2("Can't send poke 0x%08X\n", info);
									loop = 0 ; // Just quit on error
								}
								break;
							case '#':
								cmd += 2 ;
								log2("%s\n", &cmdp[cmd]);
							case '/':
								// skip comments
								res -- ;
								break ;
								
							default:
								log2("Unknown command type '%c'\n",cmdp[cmd]);
								break;
							}
							// kind of readline
							while (cmdp[++cmd] != '\0' && cmd < cmdfile.size);
							// point to next line
							while (cmdp[++cmd] == '\0' && cmd < cmdfile.size);
							res ++ ;
						}
					}
				}
				// Free used memory buffer
				if (current.content != NULL)
					free((void *)current.content);
				if (current.filename != NULL)
					free((void *)current.filename);
			}
		}
	}
	
	if (err>=0) {
		log1("Releasing usb interface\n");
		err = usb_release_interface (handle, 0);
		if (err < 0)
			log2("Error in usb_release_interface (%d)\n",err);
		log1("Released usb interface\n");
	}
	
	return err ;
}

enum {
	ARG_PROGNAME,
	ARG_2NDFILE,
	ARG_CMDFILE,
	NUM_ARGS
};

int main (int argc, char *argv[])
{
	int	size;
	btime = (double) times(NULL);
	ticks = (double) sysconf(_SC_CLK_TCK) / 1000 ;
	
	if (argc < NUM_ARGS)
	{
		log2("Usage: %s <2nd_boot_file> <command_file>\n", argv[ARG_PROGNAME]);
		return 1;
	}
	
	memset (buffer, 0, 128);
	
	u_int32_t	*p= (u_int32_t *) buffer;
	p[0x00]= cpu_to_le32 (0xf0030002);
	
	int	fd= open (argv[ARG_2NDFILE], O_RDONLY);
	if (fd < 0)
	{
		log1("open 2nd boot file\n");
		return 1;
	}
	else
	{
		size= read (fd, &buffer[128], MAX_SIZE);
		if (size < 0)
		{
			log1("read 2nd boot file\n");
			close (fd);
			return 1;
		}
		
		close (fd);
		if (size < 1024)
		{
			log2("2ndfile is too small! (%d bytes)\n", size);
			return 1;
		}
	}
	
	size= round_up (size, 4);
	
	p[0x21]= cpu_to_le32 (size - 0x40);
	
	p[0x01]= cpu_to_le32 (size);
	p[0x02]= cpu_to_le32 (size);
	p[0x03]= cpu_to_le32 (size);
	p[0x10]= cpu_to_le32 (size);
	p[0x11]= cpu_to_le32 (size);
	p[0x12]= cpu_to_le32 (size);
	
	buffsize= 128 + round_up (size, 4);
	
	cmdfile = readfile(argv[ARG_CMDFILE]);
	if (cmdfile.content == NULL)
	{
		log1("open cmdfile\n");
		return 1;
	} else {
		int i = 0 ;
		char *cmdbuffer = (char *)cmdfile.content ;
		do {
			if (cmdbuffer[i] == '\n' || cmdbuffer[i] == '\r' || cmdbuffer[i] == ';')
				cmdbuffer[i] = '\0' ;
			// Skip comment line
			if (cmdbuffer[i] == '/') {
				while ( i++ < cmdfile.size ) {
					if (cmdbuffer[i] == '\n' || cmdbuffer[i] == '\r') {
						i-- ;
						break ;
					}
				}
			}
		} while ( i++ < cmdfile.size );
	}
	
	usb_init ();
	usb_find_busses ();
	
	log1("Try to found OMAP730-USB connection\n");
	
	int	n= usb_find_devices ();
	if (n)
	{
		log1("Searching OMAP730-USB connection...\n");
		struct usb_bus	*bus;
		for (bus= usb_get_busses (); bus; bus= bus->next)
		{
			struct usb_device	*dev;
			
			for (dev= bus->devices; dev; dev= dev->next)
			{
				usb_dev_handle	*handle;
				if (dev->descriptor.idVendor == OMAP_VENDOR  &&
					dev->descriptor.idProduct == OMAP_PRODUCT  &&
					(handle= usb_open (dev)) != NULL)
				{
					log1("Found OMAP730-USB connection\n");
					int res= process (handle);
					usb_close (handle);
					if (res == 0) {
						log1("OMAP730-USB connection processed\n");
						return 0 ;
					} else
						log1("OMAP730-USB processing failed\n");
				}
			}
		}
		
	} else {
		log1("No USB bus found\n");
	}
	
	// Free memory
	if (cmdfile.content!=NULL)
		free((void *)cmdfile.content);
	
	log1("No target connection found\n");
	return 1 ;
}