/*
 * To change this license header, choose License Headers in Project Properties.
 * To change this template file, choose Tools | Templates
 * and open the template in the editor.
 */
#include "srec.h"

uint8_t h2i(uint8_t *c)
{
	int i;
	uint8_t t, b = 0;

	for (i=0; i < 2; i++)
	{
		t = c[i];
		if (t >= 'A')
			t = t - 'A' + 10;
		else
			t -= '0';

		b = (b << 4) | t;
	}
	return b;
}

int srec_decode(srec_t *pRec, uint8_t *pBuf, int buflen)
{
	uint8_t chksum, byte;
	int alen, i;

	if (!buflen)
		return srec_err;

	pRec->type = srec_rec;
	alen = 0;
	if ((pBuf[1] > '0') && (pBuf[1] < '4'))
	{
	  alen = pBuf[1] - '0' + 1;
	  pRec->type = srec_data;
	}
	else if ((pBuf[1] >= '7') && (pBuf[1] <= '9'))
	{
	  alen = 11 - (pBuf[1] - '0');
	  pRec->type = srec_eob;
	}
	else if (pBuf[1] == '0')
	{
	  alen = 2;
	  pRec->type = srec_sob;
	}

	byte = h2i(&pBuf[2]);
	pRec->size = byte;
	chksum = byte;

	pRec->addr = 0;
	for (i=0; i < alen; i++)
	{
		byte = h2i(&pBuf[4+2*i]);
        pRec->addr = pRec->addr << 8 | byte;
		chksum += byte;
	}
	pRec->size -= (alen+1);

	pRec->data = (uint8_t*) &pBuf[4 + 2*alen];
	for (i=0; i < (int)pRec->size; i++)
	{
		byte = h2i(&pRec->data[2*i]);
		pRec->data[i] = byte;
		chksum += byte;
	}
	chksum = ~chksum;
	byte = h2i(&pRec->data[2*i]);
	if (chksum != byte)
		return srec_err;

	return pRec->type;
}