#define _INTELHEX
#define MIN(a,b) (((a>b)?b:a))
#define MAX(a,b) (((a<b)?b:a))
#include "intelhex.h"

/*#define DEBUG*/

#ifdef DEBUG
extern FILE *fpdebug;
#endif

void mystrncpy(char *dest,char *src,int n)
{
	/* Like the normal 'strncpy()' but appends a '\0' at end */

	while(n--)
		*dest++=*src++;
	*dest='\0';
}

char *readintelhex(FILE *fp,unsigned char *buffer,int maxbuffer,long *plength,int flag,long *p_epromstart)
{
	/* Read a file in 'intelhex'-Format. Decode the data, check the 
	 * Checksum, fill the buffer with data.
     * IF 'flag'==CONTINOUS no fragmented data is allowed,
     *						plength is set to real read number of bytes
     * IF 'flag'==FRAGMENTED plength is set to size of part of used buffer
	 *
	 * RC: NULL if all went fine, else
	 *     Pointer to an error Message
	 */

	char line[BUFSIZ],	/* ASCII Buffer for reading line */
		 tmpbuf[5],		/* Buffer to extract and convert hex numbers */
		 *dummy;		/* like the name */
	static char errormsg[256];	/* buffer for Error Message */
	int ndata,			/* number of Databytes */
		data,			/* one databyte */
		address,		/* address for data */
		status,			/* status of intelhex line */
		chksum,			/* checksum of intelhex line */
		sum,			/* own calculated checksum */
		lnr=0,			/* linenumber */
		totalbytes=0,	/* total bytes written to buffer */
		highaddr,		/* determine the highest used buffer address */
		lowaddr,		/* determine the lowest used buffer address */
		
		i;

		/* temp. Pointer to Binary Databuffer */
	unsigned char *pbuf=(unsigned char *)-1;

#ifdef DEBUG
fprintf(fpdebug,"readintelhex()\n");
#endif

		/* set to ERROR */
	*plength=0;

	while(fgets(line,BUFSIZ,fp))
	{
		lnr++;
#ifdef DEBUG
fprintf(fpdebug,"readih: line:%d\n",lnr);
#endif

		if(line[IH_COLON]!=':')
		{
			if(lnr==1) /* First line ? */
				sprintf(errormsg,"No IntelHex Format");
			else
				sprintf(errormsg,"Line:%d  Expected ':' at first character, get '%c'\nIs the File in IntelHex Format ?\n",lnr,line[IH_COLON]);
			return(errormsg);
		}

		/* get number of databytes */
		mystrncpy(tmpbuf,&line[IH_NRDATA],2);
		ndata = strtol(tmpbuf,&dummy,16);

		/* get address */
		mystrncpy(tmpbuf,&line[IH_ADDRESS],4);
		address = strtol(tmpbuf,&dummy,16);

		/* note used buffer range */
		if(lnr==1)
		{	/* Initialize */ 
			highaddr=address+ndata-1;
			lowaddr=address;
		}
		else
		{
			lowaddr = MIN(lowaddr,address);
			highaddr= MAX(highaddr,address+ndata-1);
		}

/*printf("lnr:%d, addr:%x low:%x high:%x ndata=%d\n",lnr,address,lowaddr,highaddr,ndata);*/

		/* get status */
		mystrncpy(tmpbuf,&line[IH_STATUS],2);
		status = strtol(tmpbuf,&dummy,16);
		
		/* get chksum */
		mystrncpy(tmpbuf,&line[IH_FIRSTDATA+2*ndata],2);
		chksum = strtol(tmpbuf,&dummy,16);
		
		/* build first part of own checksum */
		sum = ndata + (0xFF&(address>>8)) + (0xFF&address) + status;

		/* check if last intelhex line */
		if(ndata==0)
		{
#ifdef DEBUG
printf("ndata=%d status=%d chksum=%d totalbytes=%d\n",ndata,status,chksum,totalbytes);
#endif

			if( (status!=1) || (chksum!=0xFF))
			{
				sprintf(errormsg,"Line:%d  ndata=0 but status=%d\n",lnr,status);
				return(errormsg);
			}
			else
			{
				/* set total written bytes */

/*printf("end: total:%d calc:%d\n",totalbytes,highaddr-lowaddr+1);*/
				*plength = (flag==CONTINOUS) ? totalbytes : highaddr-lowaddr+1 ;
				*p_epromstart = lowaddr;
				return(NULL);
			}
		}
		
		/* check if is status correct */
		if(status!=0)
		{
			sprintf(errormsg,"Line:%d  Wrong status, expected 0 not %d",lnr,status);
			return(errormsg);
		}
		
		/* check if is internal buffer big enough */
		if(address+ndata>maxbuffer)
		{
			sprintf(errormsg,"Line:%d  Too much data, internal Buffer max:%d\n",lnr,maxbuffer);
			return(errormsg);
		}

		if(pbuf!=(unsigned char *)-1)
		{
			if( (flag==CONTINOUS) && (pbuf!=buffer + address) )
			{
				sprintf(errormsg,"Line:%d  Data not continous",lnr);
				return(errormsg);
			}
		}
		i=IH_FIRSTDATA;				/* first databyte in intelhex line */
		pbuf = buffer + address;	/* first buffer position */
		while(ndata--)
		{	
			mystrncpy(tmpbuf,&line[i],2);
			data = strtol(tmpbuf,&dummy,16);
			*pbuf++=(unsigned char)data;
			i+=2;
			sum+=data;
			totalbytes++;
		}

			/* make chksum complete */
		sum = 0xFF & ( 0x100 - sum );

		/* compare checksum's */
		if(chksum != sum)
		{
			sprintf(errormsg,"Line:%d  Checksum differs, read:%d calculate:%d\n",lnr,chksum,sum);
			return(errormsg);
		}
		
	}

	if(feof(fp))
	{
		sprintf(errormsg,"Line:%d No END Status found",lnr);
		return(errormsg);
	}

	sprintf(errormsg,"Line:%d  Error reading File",lnr);
	return(errormsg);
}

char *itoh(unsigned char byte,char *pbuf)
{
	/* Convert 'byte' in ASCII Hex Notation.
	 * 
	 * Write ASCII Hex Code (2 Chars) at '*pbuf'.
	 *
	 * RC: pbuf+2    (points to next free char 
     */  

	unsigned char 	low,	/* low 4 Bits of 'byte' */
			high;	/* high 4 Bits of 'byte' (normalized) */

	low = 0x0F&byte;
/*	high = (unsigned char)(((int)(0xF0&byte))>>4);*/
	high = (unsigned char)(0xF0&byte)>>4;

	/* convert Integer to ASCII Hex */
	*pbuf++= (high<=9) ? high+'0' : (high-0x0A)+'A';
	*pbuf++= (low<=9) ? low+'0' : (low-0x0A)+'A';

	return(pbuf);
}

char *writeintelhex(FILE *fp,unsigned char *buffer,int offset,int length)
{
	/* Write a file in 'intelhex'-Format. 
	 * Code the binary data:
	 *  Determine Address, calculate checksum, write intelhex line.
	 *
	 * RC: NULL if all went fine, else
	 *     Pointer to an error Message
	 */

	char line[BUFSIZ],	/* ASCII Buffer for writing line */
		 *pos;			/* used for filling intelhex line */
	static char errormsg[256];	/* buffer for Error Message */
	int ndata,			/* number of Databytes */
		data,			/* one databyte */
		address,		/* address for data */
		status,			/* status of intelhex line */
		chksum,			/* checksum of intelhex line */
		lnr=0;			/* linenumber */


	address=offset;		/* set logically startaddress */
	status=0;			/* set status */
	line[IH_COLON]=':'; /* set Start Marker */
	
	
	while(length)
	{
		lnr++;		/* linenumber counting */

			/* write maximal IH_MAXDATA to one intelhex line */
		ndata=MIN(IH_MAXDATA,length);

			/* remaining data */
		length-=ndata;

			/* build first part of checksum */
		chksum = ndata + (0xFF&(address>>8)) + (0xFF&address) + status;
		
		pos = &line[1]; /* points to second byte of line */

		pos = itoh((unsigned char)ndata,pos); 					/* Write data counter */
		pos = itoh((unsigned char)(address>>8),pos);  /* Write High Byte Address */
		pos = itoh((unsigned char)(0xFF&address),pos); /* Write Low Byte Address */
		pos = itoh((unsigned char)status,pos); 				/* Write status */

			/* calculate address for next line */
		address+=ndata;

		while(ndata--)
		{
			data=*buffer;		
			buffer++;		
			chksum+=data;						/* adding for chksum */

			pos = itoh((unsigned char)data,pos);			/* Write databyte */
		}

		chksum=0xFF&(0x100-(0xFF&chksum));
		pos = itoh(chksum,pos);					/* Write chksum */
		*pos++ = '\n';							/* Close String */
		*pos = '\0';							/* Close String */

		if(fputs(line,fp)<0)
		{
			sprintf(errormsg,"Line:%d Can't write line",lnr);
			return(errormsg);
		}
	}
	fputs(":00000001FF\n",fp);

	return(NULL);

}