Imanol-Mikel Barba Sabariego / smartcities · Files

#include "ntp.h"

#define DBG(fmt,...)          		if(1){printf("[NTP]         "fmt"\r\n", ##__VA_ARGS__);}else{({});}
#define DBG_WARNING(fmt,...)        if(1){printf("[NTP_WARNING] "fmt"\r\n", ##__VA_ARGS__);}else{({});}

//define LEAP_YEAR(Y) (((1970+Y)>0) && !((1970+Y)%4) && (((1970+Y)%100) || !((1970+Y)%400)))
//const unsigned long seventyYears = 2208988800UL;
//const int timeZone = 2; //Time offset from GMT
//unsigned long daysPerMonth[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};
unsigned long secsSince1900=0;
int flag=0;

struct udp_pcb* udpConnectionHandler= NULL;
uint8_t 		udpNtpOnline 		= 0;


/*----------------------------------------------------------------------------------------------------------------------------------------
INITIALIZATION/PROCESSING FUNCTIONS
-----------------------------------------------------------------------------------------------------------------------------------------*/
void udpNtp_init(){
	udpConnectionHandler= NULL;
	udpNtpOnline		= 0;
	secsSince1900=0;
	flag=0;
}

/*
* Check if the connection handler has been initialized,
* and if so send a new message
*/
void udpNtp_process(){
	while(udpNtpOnline==0){
		DBG("TX abandonned! [Not connected]\r\n");
		chThdSleepMilliseconds(500);
	}
	udpNtp_send();
}

/*----------------------------------------------------------------------------------------------------------------------------------------
UDP TRANSMISSION HANDLING FUNCTIONS
-----------------------------------------------------------------------------------------------------------------------------------------*/
/*
* Sends a new message to the remote peer
*/
void udpNtp_send(){
	uint16_t remotePort 			= 123;
	char packetBuffer[NTP_PACKET_LENGTH];
	static uint32_t messageCount 	= 0;
	memset (packetBuffer,0,NTP_PACKET_LENGTH);
	packetBuffer[0] =0x1B;//0b11100011;   // LI, Version, Mode
  	packetBuffer[1] = 0;     		// Stratum, or type of clock
  	packetBuffer[2] = 6;     		// Polling Interval
  	packetBuffer[3] = 0xEC;  		// Peer Clock Precision
  	// 8 bytes of zero for Root Delay & Root Dispersion
  	packetBuffer[12]  = 49;
  	packetBuffer[13]  = 0x4E;
  	packetBuffer[14]  = 49;
  	packetBuffer[15]  = 52;
	struct pbuf *pbuffer;
	ip_addr_t addr;
	uint32_t ipaddr;
	
	err_t err;
	
	//Convert the IP address from string to lwip format
	ipaddr = ipaddr_addr(NTP_SERVER);
	ip4_addr_set_u32((&addr), ipaddr);
	
	/*
	* Allocate a pbuf which we are going to fill with the transmitted data.
	* We use libwismart_LwIP_lock() here because we are making a LWIP call
	*/
libwismart_LwIP_lock();
	pbuffer = pbuf_alloc(PBUF_TRANSPORT, NTP_PACKET_LENGTH, PBUF_POOL);
libwismart_LwIP_unlock();
	if(pbuffer == NULL){
		DBG_WARNING("pbuf_alloc() FAILED!\r\n");
		return;
	}

	/*
	* Fill the payload of the pbuf with the message to be transmitted
	* NOTE: Because pbuffer->payload has type (void*), we have to cast it (else we may have undefined behaviour)
	*/
	memcpy((uint8_t*)pbuffer->payload, packetBuffer,  NTP_PACKET_LENGTH);
	
	/*
	*	Send the packet and release the allocated pbuf.
	* 	We use libwismart_LwIP_lock() here because we are making a LWIP call
	*/
	libwismart_LwIP_lock();
    err = udp_sendto(udpConnectionHandler, pbuffer, &addr, remotePort);
	libwismart_LwIP_unlock();

	/* 
	* Release the allocated pbuf 
	* We use libwismart_LwIP_lock() here because we are making a LWIP call
	*/
	libwismart_LwIP_lock();
	pbuf_free(pbuffer);
	libwismart_LwIP_unlock();

	if(err != ERR_OK){
		DBG_WARNING("udp_sendto() FAILED!");
		return;
	}

	DBG("Message #%u sent!",messageCount++);
}

/*----------------------------------------------------------------------------------------------------------------------------------------
UDP RECEPTION HANDLING FUNCTIONS
-----------------------------------------------------------------------------------------------------------------------------------------*/
void udpNtp_dataReceivedCb(void * arg, struct udp_pcb * upcb,struct pbuf * firstPbuf,struct ip_addr * addr, u16_t port){
    unsigned char   *payload;
    unsigned int    payloadLen;
    struct pbuf 	*currentPbuf;
	
	currentPbuf = firstPbuf;
	while(currentPbuf != NULL){
	
		/*
		* It is important to cast the pbufNow->payload because its type is (void*)
		*/
		payload 	= (unsigned char *)currentPbuf->payload;
        payloadLen 	= currentPbuf->len;
		
		DBG("New message [%u bytes] received!\r\n", payloadLen);

		currentPbuf = currentPbuf->next;
	};

	uint32_t temp=read32(payload, 40);
	secsSince1900=(unsigned long) temp;
	flag=1;
	
	/* 
	* Free the pbuf list.
	*/
    pbuf_free(firstPbuf);
	
}

/*----------------------------------------------------------------------------------------------------------------------------------------
UDP CONNECTION SETUP FUNCTIONS
-----------------------------------------------------------------------------------------------------------------------------------------*/
/*
* Setups a new udp connection handler
*/
uint32_t udpNtp_setupConnection(){
	uint32_t error;
	
libwismart_LwIP_lock();
	error = udpNtp_setupConnectionL();
libwismart_LwIP_unlock();

	return error;
}

/*
* Setups a new udp connection handler (Locked version of udpNTP_setupConnection)
*/
uint32_t udpNtp_setupConnectionL(){
	uint16_t localPort = 5004;
	err_t err;
	
    /* 
	* Create a new udp connection handler
	*/
	udpConnectionHandler = udp_new();
	if(udpConnectionHandler == NULL){
        DBG_WARNING("udp_new() FAILED!\r\n");
		return 1;
    }
	
	/*
	* Bind the connection to a local port. By giving 0 
	* as third argument(port), you can say to lwip to bind the connection
	* to any available port of our system.
	*/
    err = udp_bind(udpConnectionHandler, IP_ADDR_ANY, localPort);
	if(err != ERR_OK){
        DBG_WARNING("udp_bind() FAILED!\r\n");
		udp_remove(udpConnectionHandler);
		return 2;
    }
	/* 
	* Set the callback function when udp data are received.
	* Since we only transmit, we should set this to NULL(second argument).
	* For demonstration reasons only we provide a callback function
	* that will be called each time a packet is received at port 'localPort'
	*/
	udp_recv(udpConnectionHandler, udpNtp_dataReceivedCb, NULL);

	udpNtpOnline = 1;
	
	return 0;
}

uint32_t read32(char* buffer, int offset) {
    char b0 = buffer[offset];
    char b1 = buffer[offset+1];
    char b2 = buffer[offset+2];
    char b3 = buffer[offset+3];

    // convert signed bytes to unsigned values
    uint32_t i0 = ((b0 & 0x80) == 0x80 ? (b0 & 0x7F) + 0x80 : b0);
    uint32_t i1 = ((b1 & 0x80) == 0x80 ? (b1 & 0x7F) + 0x80 : b1);
    uint32_t i2 = ((b2 & 0x80) == 0x80 ? (b2 & 0x7F) + 0x80 : b2);
    uint32_t i3 = ((b3 & 0x80) == 0x80 ? (b3 & 0x7F) + 0x80 : b3);

    uint32_t v = (i0 << 24) + (i1 << 16) + (i2 << 8) + i3;
    return v;
}

unsigned long getSecsSince1900(){
	while(flag==0){
		DBG("in function getSecsSince1900: waiting for NTP procces\n");
		chThdSleepMilliseconds(500);
	}
	return secsSince1900;
}

Date getDate(){
	while(flag==0){
		DBG("in function getDate: waiting for NTP procces\n");
		chThdSleepMilliseconds(500);
	}


	const unsigned long seventyYears = 2208988800UL;
	const int timeZone = 2; //Time offset from GMT
	unsigned long daysPerMonth[] = {31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31};

	unsigned long epoch = secsSince1900 - seventyYears;

	int second = epoch % 60;
	epoch /= 60;
	int minute = epoch % 60;
	epoch /= 60;
	int hour = ((epoch % 24) + timeZone) % 24;
	epoch /= 24;
	int year = 0;
	unsigned long days = 0;

	while((unsigned)(days += (LEAP_YEAR(year) ? 366 : 365)) <= epoch){
		year++;
	}
	days -= LEAP_YEAR(year) ? 366 : 365;
	epoch -= days;

	int j=0;
	int cont=0;
	int aux=epoch;
	if(LEAP_YEAR(year)){
		daysPerMonth[1]=29;
	}

	while (daysPerMonth[j]<sizeof(daysPerMonth)){

		cont+=daysPerMonth[j];
		if(epoch<cont){
			break;
		}
		aux-=daysPerMonth[j];
		j++;
	}

	int day=++aux;
	int month=++j;
	year+=1970;

	Date date;
	date.second=second;
	date.minute=minute;
	date.hour=hour;
	date.day=day;
	date.month=month;
	date.year=year;

	return date;
}

void udpNTP_Setup(){
	DBG("start_init\r\n");
	udpNtp_init();

	DBG("start_Setup_Connection\r\n");
	udpNtp_setupConnectionL();

	DBG("start_Procces\r\n");
	udpNtp_process();
}

char* timestamp_data(char* value,Date time)
{
	uint8_t length = strlen(value) + strlen(",00/00/0000T00:00:00") + 1;
	char str_day[3],str_month[3],str_year[5],str_hour[3],str_minute[3],str_second[3];
	char* data = chHeapAlloc(NULL,length*sizeof(char));

	sprintf(str_day,"%d",time.day);
	sprintf(str_month,"%d",time.month);
	sprintf(str_year,"%d",time.year);
	sprintf(str_hour,"%d",time.hour);
	sprintf(str_minute,"%d",time.minute);
	sprintf(str_second,"%d",time.second);

	strcpy(data,value);
	strcat(data,",");
	strcat(data, str_day);
	strcat(data,"/");
	strcat(data, str_month);
	strcat(data,"/");
	strcat(data, str_year);
	strcat(data,"T");
	strcat(data, str_hour);
	strcat(data,":");
	strcat(data, str_minute);
	strcat(data,":");
	strcat(data, str_second);
	data[length-1] = '\0';
	return data;
}


/*Funcion para testeo del resto de funciones relacionadas con NTP. En el main() el orden de ejecución debe ser el siguiente.
1ro: udpNTP_Setup
2do: getSecsSince1900() o getDate()
si se quiere actualizar la fecha actual, debemos llamar primeramente a UdpNTP_Setup.*/
/*void udpNtp_test(){

	udpNTP_Setup();
	//unsigned long var1=getSecsSince1900();
	Date var2=getDate();

	//DBG("seconds since 1900: %lu\r\n", var1);

	DBG("Hora:%i:%i:%i\r\n", var2.hour, var2.minute, var2.second);
	DBG("Fecha:%i|%i|%i\r\n", var2.day,var2.month,var2.year);
}*/