Fix: timer.* issues

Change-Id: I1c3721403198b3c5525a811bd3c7cbf6b8e78e5b
Signed-off-by: Romain Forlot <romain.forlot@iot.bzh>

10 files changed, 238 insertions, 273 deletions

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index a034b433..84e8ecf0 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -65,7 +65,7 @@ link_libraries(${EXTRAS_LIBRARIES})
 
 message(STATUS "Creation of ${PROJECT_NAME} binding for AFB-DAEMON")
 ###########################################################################
-add_library(${PROJECT_NAME}-binding MODULE ${PROJECT_NAME}-binding.cpp)
+add_library(${PROJECT_NAME}-binding MODULE ${PROJECT_NAME}-binding.cpp can-signals.cpp)
 
 set_target_properties(${PROJECT_NAME}-binding PROPERTIES
 	PREFIX ""
diff --git a/src/can-signals.cpp b/src/can-signals.cpp
index 610a5a8d..b2996933 100644
--- a/src/can-signals.cpp
+++ b/src/can-signals.cpp
@@ -26,9 +26,6 @@
 std::map <CanSignal, struct afb_event> subscribed_signals;
 std::map <CanSignal, struct afb_event>::iterator subscribed_signals_i;
 
-/* Find one or many signals based on its name or id
-* passed through openxc_DynamicField.
-*/
 #define MESSAGE_SET_ID 0
 std::vector <CanSignal> find_can_signals(openxc_DynamicField &key)
 {
diff --git a/src/can-signals.hpp b/src/can-signals.hpp
index 91a6932b..27205137 100644
--- a/src/can-signals.hpp
+++ b/src/can-signals.hpp
@@ -69,16 +69,14 @@ int getMessageCount();
  *	 */
 CanBus* getCanBuses();
 
-/* Public: Decode CAN signals from raw CAN messages, translate from engineering
- *	* units to something more human readable, and send the resulting value over USB
- *	 * as an OpenXC-style JSON message.
- *	  *
- *	   * This is the main workhorse function of the VI. Every time a new
- *		* CAN message is received that matches one of the signals in the list returend
- *		 * by getSignals(), this function is called with the message ID and 64-bit data
- *		  * field.
- *		   *
- *			* bus - The CAN bus this message was received on.
- *			 * message - The received CAN message.
- *			  */
-void decodeCanMessage(openxc::pipeline::Pipeline* pipeline, CanBus* bus, CanMessage* message);
+
+/**
+ * @brief Find one or many signals based on its name or id
+ * passed through openxc_DynamicField.
+ *
+ * params[openxc_DynamicField&] - a const reference with the key to search into signal.
+ * Key is either a signal name or its CAN arbitration id.
+ *
+ * return[std::vector<CanSignal>] return found CanSignal array.
+ */
+std::vector <CanSignal> find_can_signals(const openxc_DynamicField &key)
\ No newline at end of file
diff --git a/src/can-utils.cpp b/src/can-utils.cpp
index 3aa2ca88..8d5f0252 100644
--- a/src/can-utils.cpp
+++ b/src/can-utils.cpp
@@ -212,69 +212,72 @@ void can_bus_t::start_threads()
  * params[std::ifstream& conf_file] conf_file ifstream to the JSON configuration 
  * file located at the rootdir of the binding
  */
- void init_can_dev()
- {
-   std::vector<std::string> devices_name;
-   int i, t;
-   
-   devices_name = read_conf(conf_file_);
-   
-   t = devices_name.size();
-   i=0
-   
-   for(const auto& device : devices_name)
-   {
-     can_bus_dev_t(device);
-     i++;
-   }
-   
-   NOTICE(interface_, "Initialized %d/%d can bus device(s)", i, t);
- }
+int init_can_dev()
+{
+	std::vector<std::string> devices_name;
+	int i, t, ret;
+
+	devices_name = read_conf(conf_file_);
+
+	if (devices_name)
+	{
+		t = devices_name.size();
+		i=0
+
+		for(const auto& device : devices_name)
+		{
+			can_bus_dev_t(device);
+			i++;
+		}
+
+		NOTICE(interface_, "Initialized %d/%d can bus device(s)", i, t);
+		return 0;
+	}
+	ERROR(interface_, "init_can_dev: Error at CAN device initialization.");
+	return 1;
+}
 
 /** 
  * @brief Read the conf file and extract device name
  * 
- * @params[std::ifstream& conf_file] conf_file JSON configuration
- * file located at the rootdir of the binding
- * 
  * @return[std:vector<std::string>] return a vector of device name
  */
- std::vector<std::string> read_conf(std::ifstream& conf_file)
- {
-  std::vector<std::string> ret;
-  std::string fd_conf_content;
+std::vector<std::string> read_conf()
+{
+	std::vector<std::string> ret;
+	std::string fd_conf_content;
 	json_object jo, canbus;
-  int n, i, ok;
-  
+	int n, i, ok;
+
 	/* Open JSON conf file */
-	if (conf_file)
+	if (conf_file_)
 	{
-		conf_file.seekg(0, std::ios::end);
-		conf_file.resize(conf_file.tellg());
-		conf_file.seekg(0, std::ios::beg);
-		conf_file.read(&fd_conf_content[0], fd_conf_content.size());
-		conf_file.close();
-
-  	jo = json_tokener_parse(&fd_conf_content);
-  
-    if (jo == NULL || !json_object_object_get_ex(&jo, "canbus", &&canbus))
-      ERROR(interface_, "Can't find canbus node in the configuration file. Please review it.");
-    else if (json_object_get_type(canbus) != json_type_array)
-  		ret.push_back(json_object_get_string(a));
-  	else
-  	{
-  		n = json_object_array_length(a);
-  		ok = 0;
-  		for (i = 0 ; i < n ; i++)
-  			ret.push_back(json_object_get_string(json_object_array_get_idx(a, i)));
-    }
-    return ret;
+		conf_file_.seekg(0, std::ios::end);
+		conf_file_.resize(conf_file_.tellg());
+		conf_file_.seekg(0, std::ios::beg);
+		conf_file_.read(&fd_conf_content[0], fd_conf_content.size());
+		conf_file_.close();
+
+		jo = json_tokener_parse(&fd_conf_content);
+
+		if (jo == NULL || !json_object_object_get_ex(&jo, "canbus", &&canbus))
+		{
+			ERROR(interface_, "Can't find canbus node in the configuration file. Please review it.");
+			ret = nullptr;
+		}
+		else if (json_object_get_type(canbus) != json_type_array)
+			ret.push_back(json_object_get_string(a));
+		else
+		{
+			n = json_object_array_length(a);
+			ok = 0;
+			for (i = 0 ; i < n ; i++)
+			ret.push_back(json_object_get_string(json_object_array_get_idx(a, i)));
+		}
+	return ret;
 	}
-  else
-  {
-    ERROR(interface_, "Problem at reading the conf file");
-    return 0;
-  }
+	ERROR(interface_, "Problem at reading the conf file");
+	return nullptr;
 }
 
 /**
diff --git a/src/can-utils.hpp b/src/can-utils.hpp
index 8b64cb3b..db97b4bf 100644
--- a/src/can-utils.hpp
+++ b/src/can-utils.hpp
@@ -62,6 +62,55 @@ typedef openxc_DynamicField (*SignalDecoder)(struct CanSignal* signal,
 typedef uint64_t (*SignalEncoder)(struct CanSignal* signal,
 		openxc_DynamicField* value, bool* send);
 
+/* Public: The ID format for a CAN message.
+ *
+ * STANDARD - standard 11-bit CAN arbitration ID.
+ * EXTENDED - an extended frame, with a 29-bit arbitration ID.
+ */
+enum CanMessageFormat {
+	STANDARD,
+	EXTENDED,
+};
+typedef enum CanMessageFormat CanMessageFormat;
+
+/* A compact representation of a single CAN message, meant to be used in in/out
+ * buffers.
+ *
+ * id - The ID of the message.
+ * format - the format of the message's ID.
+ * data  - The message's data field.
+ * length - the length of the data array (max 8).
+struct CanMessage {
+	uint32_t id;
+	CanMessageFormat format;
+	uint8_t data[CAN_MESSAGE_SIZE];
+	uint8_t length;
+};
+typedef struct CanMessage CanMessage;
+*/
+class can_message_t {
+	private:
+		afb_binding_interface interface_;
+		uint32_t id_;
+		CanMessageFormat format_;
+		uint8_t data_[CAN_MESSAGE_SIZE];
+		uint8_t length_;
+
+	public:
+		uint32_t get_id() const;
+		int get_format() const;
+		uint8_t get_data() const;
+		uint8_t get_lenght() const;
+
+		void set_id(uint32_t id);
+		void set_format(CanMessageFormat format);
+		void set_data(uint8_t data);
+		void set_lenght(uint8_t length);
+
+		void convert_from_canfd_frame(canfd_frame frame);
+		canfd_frame convert_to_canfd_frame();
+};
+
 /** 
  * @brief Object representing a can device. Handle opening, closing and reading on the
  * socket. This is the low level object to be use by can_bus_t.
@@ -91,8 +140,7 @@ class can_bus_dev_t {
 		can_message_t* next_can_message();
 		void push_new_can_message(const can_message_t& can_msg);		
 		bool has_can_message() const;
-}
-
+};
 
 /** 
  * @brief Object used to handle decoding and manage event queue to be pushed.
@@ -112,11 +160,11 @@ class can_bus_t {
 		std::queue <openxc_VehicleMessage> vehicle_message_q_;
 
 	public:
+		int init_can_dev();
+		std::vector<std::string> read_conf();
+		
 		void start_threads();
 		
-		void init_can_dev(std::ifstream& conf_file);
-		std::vector<std::string> read_conf()
-
 		int send_can_message(can_message_t can_msg);
 
 		openxc_VehicleMessage& next_vehicle_message();
@@ -124,57 +172,6 @@ class can_bus_t {
 		bool has_vehicle_message() const;
 };
 
-/* A compact representation of a single CAN message, meant to be used in in/out
- * buffers.
- *
- * id - The ID of the message.
- * format - the format of the message's ID.
- * data  - The message's data field.
- * length - the length of the data array (max 8).
-struct CanMessage {
-	uint32_t id;
-	CanMessageFormat format;
-	uint8_t data[CAN_MESSAGE_SIZE];
-	uint8_t length;
-};
-typedef struct CanMessage CanMessage;
-*/
-class can_message_t {
-	private:
-		afb_binding_interface interface_;
-		uint32_t id_;
-		CanMessageFormat format_;
-		uint8_t data_[CAN_MESSAGE_SIZE];
-		uint8_t length_;
-
-	public:
-		uint32_t get_id() const;
-		int get_format() const;
-		uint8_t get_data() const;
-		uint8_t get_lenght() const;
-
-		void set_id(uint32_t id);
-		void set_format(CanMessageFormat format);
-		void set_data(uint8_t data);
-		void set_lenght(uint8_t length);
-
-		void convert_from_canfd_frame(canfd_frame frame);
-		canfd_frame convert_to_canfd_frame();
-};
-
-QUEUE_DECLARE(can_message_t, 8);
-
-/* Public: The ID format for a CAN message.
- *
- * STANDARD - standard 11-bit CAN arbitration ID.
- * EXTENDED - an extended frame, with a 29-bit arbitration ID.
- */
-enum CanMessageFormat {
-	STANDARD,
-	EXTENDED,
-};
-typedef enum CanMessageFormat CanMessageFormat;
-
 /* Public: A state encoded (SED) signal's mapping from numerical values to
  * OpenXC state names.
  *
@@ -346,17 +343,11 @@ typedef void (*CommandHandler)(const char* name, openxc_DynamicField* value,
  * genericName - The name of the command.
  * handler - An function to process the received command's data and perform some
  *		action.
+ */
 typedef struct {
 	const char* genericName;
 	CommandHandler handler;
 } CanCommand;
- */
-
-class CanCommand_c {
-	private:
-		const char* genericName;
-		CommandHandler handler;
-};
 
 /* Pre initialize actions made before CAN bus initialization
  *
diff --git a/src/low-can-binding.cpp b/src/low-can-binding.cpp
index 7899797b..c5fadbe3 100644
--- a/src/low-can-binding.cpp
+++ b/src/low-can-binding.cpp
@@ -33,6 +33,7 @@
 #include <functional>
 #include <memory>
 #include <thread>
+#include <fstream>
 
 #include <json-c/json.h>
 #include <openxc.pb.h>
@@ -40,14 +41,15 @@
 #include <afb/afb-binding.h>
 #include <afb/afb-service-itf.h>
 
-//#include "obd2.hpp"
+#include "obd2.hpp"
 #include "can-utils.hpp"
 #include "can-signals.hpp"
 
 /*
- *	 Interface between the daemon and the binding
+ *	Interface between the daemon and the binding
  */
 static const struct afb_binding_interface *interface;
+static obd2_handler_t obd2_handler();
 
 /********************************************************************************
 *
@@ -55,153 +57,90 @@ static const struct afb_binding_interface *interface;
 *
 *********************************************************************************/
 
-/*
- * TBF TBF TBF
- * called on an event on the CAN bus
- static int on_event(sd_event_source *s, int fd, uint32_t revents, void *userdata)
-{
-	openxc_CanMessage can_message;
-
-	can_message = openxc_CanMessage_init_default;
-
-	/* read available data */
-	/*
-	if ((revents & EPOLLIN) != 0)
-	{
-		read_can(&can_message);
-		send_event();
-	}
-*/
-	/* check if error or hangup */
-/*
-	if ((revents & (EPOLLERR|EPOLLRDHUP|EPOLLHUP)) != 0)
-	{
-		sd_event_source_unref(s);
-		close(fd);
-		connect_to_event_loop();
-	}
+/********************************************************************************
+*
+*		Subscription and unsubscription
+*
+*********************************************************************************/
 
-	return 0;
-}
-*/
-/*
- * USELESS SINCE THREADS SEPARATION
- *
- * Get the event loop running.
- * Will trigger on_event function on EPOLLIN event on socket
- *
- * Return 0 or positive value on success. Else negative value for failure.
-static int connect_to_event_loop(CanBus &CanBus_handler)
+static int subscribe_unsubscribe_signal(struct afb_req request, bool subscribe, std::vector<CanSignal>::const_iterator& sig_i)
 {
-	sd_event *event_loop;
-	sd_event_source *source;
-	int rc;
+	int ret;
 
-	if (CanBus_handler.socket < 0)
+	const auto& ss_i = subscribed_signals.find(sig_i);
+	if (ss_i != subscribed_signals.end())
 	{
-		return CanBus_handler.socket;
+		if(!afb_event_is_valid(ss_i->second))
+		{
+			if(!subscribe)
+			{
+				NOTICE(interface, "Event isn't valid, it can't be unsubscribed.");
+				ret = 1;
+			}
+			else
+			{
+				ss_i->second = afb_daemon_make_event(interface->daemon, ss_i->first.genericName);
+				if (!afb_event_is_valid(ss_i->second)) 
+				{
+					ERROR(interface, "Can't create an event, something goes wrong.");
+					ret = 0;
+				}
+			}
+		}
 	}
-
-	event_loop = afb_daemon_get_event_loop(interface->daemon);
-	rc = sd_event_add_io(event_loop, &source, CanBus_handler.socket, EPOLLIN, on_event, NULL);
-	if (rc < 0)
-	{
-		CanBus_handler.close();
-		ERROR(interface, "Can't connect CAN bus %s to the event loop", CanBus_handler.device);
-	} else
+	else
 	{
-		NOTICE(interface, "Connected CAN bus %s to the event loop", CanBus_handler.device);
+		subscribed_signals[sig_i] = afb_daemon_make_event(interface->daemon, sig_i->genericName);
+		if (!afb_event_is_valid(ss_i->second)) 
+		{
+			ERROR(interface, "Can't create an event, something goes wrong.");
+			ret = 0;
+		}
 	}
 
-	return rc;
-}
- */
-
-/********************************************************************************
-*
-*		Subscription and unsubscription
-*
-*********************************************************************************/
-
-static int subscribe_unsubscribe_signal(struct afb_req request, bool subscribe, std::vector<CanSignal>::const_iterator& sig_i)
- {
-  int ret;
-  
-  const auto& ss_i = subscribed_signals.find(sig_i);
-  if (ss_i != subscribed_signals.end())
-  {
-    if(!afb_event_is_valid(ss_i->second))
-    {
-      if(!subscribe)
-      {
-        NOTICE(interface, "Event isn't valid, it can't be unsubscribed.");
-        ret = 1;
-      }
-      else
-      {
-        ss_i->second = afb_daemon_make_event(afbitf->daemon, ss_i->first.genericName);
-        if (!afb_event_is_valid(ss_i->second)) 
-        {
-          ERROR(interface, "Can't create an event, something goes wrong.");
-          ret = 0;
-        }
-      }
-    }
-  }
-  else
-  {
-    subscribed_signals[sig_i] = afb_daemon_make_event(afbitf->daemon, sig_i.genericName);
-    if (!afb_event_is_valid(ss_i->second)) 
-    {
-      ERROR(interface, "Can't create an event, something goes wrong.");
-      ret = 0;
-    }
- }
-          
 	if (((subscribe ? afb_req_subscribe : afb_req_unsubscribe)(request, subscribed_signals[sig_i])) < 0)
 	{
-	  ERROR(interface, "Operation goes wrong for signal: %s", sig_i.genericName);
-	  ret = 0;
+		ERROR(interface, "Operation goes wrong for signal: %s", sig_i->genericName);
+		ret = 0;
 	}
-  else
-	  ret = 1;
+	else
+		ret = 1;
 	
 	return ret;
- }
+}
 
-static int subscribe_unsubscribe_signals(struct afb_req request, bool subscribe, const std:vector<CanSignal>& signals)
+static int subscribe_unsubscribe_signals(struct afb_req request, bool subscribe, const std::vector<CanSignal>& signals)
 {
-	std::vector<CanSignal>::iterator signal_i;
-	std::map <CanSignal, struct afb_event>::iterator s_signal_i;
-  int ret;
+	int ret;
 
 	// TODO: lock the subscribed_signals when insert/remove
 	for(const auto& signal_i : signals)
 	{
-	  ret = subscribe_unsubscribe_signal(request, subscribe, signal_i);
-	  if(ret == 0)
-	    return ret;
+		ret = subscribe_unsubscribe_signal(request, subscribe, signal_i);
+		if(ret == 0)
+			return ret;
 	}
 }
 
 static int subscribe_unsubscribe_all(struct afb_req request, bool subscribe)
 {
-	int i, n, e;
+	int n, e;
 
-	n = sizeof OBD2_PIDS / sizeof * OBD2_PIDS;
+	n = obd2_handler.OBD2_PIDS.size();
 	e = 0;
-	for (i = 0 ; i < n ; i++)
-		e += !subscribe_unsubscribe_sig(request, subscribe, &OBD2_PIDS[i]);
+	for (const auto& pid : obd2_handler.OBD2_PIDS)
+		e += !subscribe_unsubscribe_signals(request, subscribe, pid);
+	
 	return e == 0;
 }
 
 static int subscribe_unsubscribe_name(struct afb_req request, bool subscribe, const char *name)
 {
-	 std::vector<CanSignal> sig;
-	 int ret = 0;
+	std::vector<CanSignal> sig;
+	int ret = 0;
 
-	if (!strcmp(name, "*"))
-	  ret = subscribe_unsubscribe_all(request, subscribe);
+	if (!::strcmp(name, "*"))
+		ret = subscribe_unsubscribe_all(request, subscribe);
 	else
 	{
 		//if(obd2_handler_c.is_obd2_signal(name))
@@ -211,12 +150,12 @@ static int subscribe_unsubscribe_name(struct afb_req request, bool subscribe, co
 		}
 		else
 		{
-		sig = find_can_signals(name);
-		if (sig.empty())
-			ret = 0;
-  	}
-  	ret = subscribe_unsubscribe_signals(request, subscribe, sig);
-  }
+			sig = find_can_signals(name);
+			if (sig.empty())
+				ret = 0;
+		}
+		ret = subscribe_unsubscribe_signals(request, subscribe, sig);
+	}
 	return ret;
 }
 
@@ -258,11 +197,11 @@ static void unsubscribe(struct afb_req request)
 {
 	subscribe_unsubscribe(request, false);
 }
+
 static const struct afb_verb_desc_v1 verbs[]=
 {
-  { .name= "subscribe",    .session= AFB_SESSION_NONE, .callback= subscribe,	.info= "subscribe to notification of CAN bus messages." },
-  { .name= "unsubscribe",  .session= AFB_SESSION_NONE, .callback= unsubscribe,	.info= "unsubscribe a previous subscription." },
-	{NULL}
+	{ .name= "subscribe",    .session= AFB_SESSION_NONE, .callback= subscribe,	.info= "subscribe to notification of CAN bus messages." },
+	{ .name= "unsubscribe",  .session= AFB_SESSION_NONE, .callback= unsubscribe,	.info= "unsubscribe a previous subscription." }
 };
 
 static const struct afb_binding binding_desc = {
@@ -294,7 +233,12 @@ int afbBindingV1ServiceInit(struct afb_service service)
 	fd_conf = afb_daemon_rootdir_open_locale(interface->daemon, "can_bus.json", O_RDONLY, NULL);
 
 	/* Open CAN socket */
-	can_bus_t can_bus_handler(interface, ));
-	CanBus_handler.open();
-	CanBus_handler.start_threads();
+	can_bus_t can_bus_handler(interface, fd_conf);
+	if(can_bus_handler.init_can_dev() == 0)
+	{
+		can_bus_handler.start_threads();
+		return 0;
+	}
+
+	return 1;
 }
diff --git a/src/obd2.cpp b/src/obd2.cpp
index 3563928c..96b6c083 100644
--- a/src/obd2.cpp
+++ b/src/obd2.cpp
@@ -28,7 +28,7 @@ void shims_timer()
 /*
  * Will scan for supported Obd2 pids
  */
-obd2_handler_c::obd2_handler_c(afb_binding_interface *itf, can_bus_t cb)
+obd2_handler_t::obd2_handler_t(afb_binding_interface *itf, can_bus_t cb)
 {
 	can_bus_t can_bus = cb;
 	DiagnosticShims shims = diagnostic_init_shims(shims_logger, can_bus.send_can_message, NULL);
@@ -41,26 +41,26 @@ obd2_handler_c::obd2_handler_c(afb_binding_interface *itf, can_bus_t cb)
 	}
 }
 
-void obd2_handler_c::add_request(int pid)
+void obd2_handler_t::add_request(int pid)
 {
 	DiagnosticRequest request = {
 	arbitration_id: OBD2_FUNCTIONAL_BROADCAST_ID,
 	mode: 0x1, has_pid: true, pid: pid};
 }
 
-bool obd2_handler_c::is_obd2_request(DiagnosticRequest* request)
+bool obd2_handler_t::is_obd2_request(DiagnosticRequest* request)
 {
 	return request->mode == 0x1 && request->has_pid && request->pid < 0xff;
 }
 
-bool obd2_handler_c::is_obd2_signal(const char *name)
+bool obd2_handler_t::is_obd2_signal(const char *name)
 {
 	if(fnmatch("obd2.*", name, NULL) == 0)
 		return true;
 	return false;
 }
 
-bool obd2_handler_c::decode_obd2_response(DiagnosticResponse* responce)
+bool obd2_handler_t::decode_obd2_response(DiagnosticResponse* responce)
 {
 	return diagnostic_decode_obd2_pid(response);
 }
diff --git a/src/obd2.hpp b/src/obd2.hpp
index 72b41eb9..b5bd0f87 100644
--- a/src/obd2.hpp
+++ b/src/obd2.hpp
@@ -87,7 +87,7 @@ float handleObd2Pid(const DiagnosticResponse* response, float parsedPayload);
  * Object to handle obd2 session with pre-scan of supported pid
  * then request them regularly
  */
-class obd2_handler_c {
+class obd2_handler_t {
 	private:
 
 	public:
@@ -144,4 +144,4 @@ class obd2_handler_c {
 		* @return float number representing the requested value.
 		*/
 		bool decode_obd2_response(DiagnosticResponse* responce);
-}
\ No newline at end of file
+};
\ No newline at end of file
diff --git a/src/timer.cpp b/src/timer.cpp
new file mode 100644
index 00000000..70ce2b81
--- /dev/null
+++ b/src/timer.cpp
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2015, 2016 "IoT.bzh"
+ * Author "Romain Forlot" <romain.forlot@iot.bzh>
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *	 http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+ 
+inline unsigned long systemTimeMs()
+{
+	struct timeb t_msec;
+	unsigned long int timestamp_msec;
+	
+	if(!::ftime(&t_msec))
+	{
+		timestamp_msec = ((unsigned long int) t_msec.time) * 1000ll + 
+			(unsigned long int) t_msec.millitm;
+	}
+	return timestamp_msec;
+}
\ No newline at end of file
diff --git a/src/timer.hpp b/src/timer.hpp
index 76eb51df..798baa26 100644
--- a/src/timer.hpp
+++ b/src/timer.hpp
@@ -17,17 +17,20 @@
 
 #pragma once
 
-//typedef unsigned long (*TimeFunction)();
+#include <sys/timeb.h>
 
-/* Public: A frequency counting clock.
+typedef unsigned long (*TimeFunction)();
+
+/**
+ * @brief: A frequency counting clock.
  *
  * frequency - the clock frequency in Hz.
  * last_time - the last time (in milliseconds since startup) that the clock
  *	ticked.
- * time_function - a function returning current time in ms
+ * time_function - a function returning current time
  */
 typedef struct {
 		float frequency;
 		unsigned long lastTick;
 		TimeFunction timeFunction;
-} FrequencyClock;
+} FrequencyClock;
\ No newline at end of file