Fix unaligned access exception in gptp, add SW hook (#22)

* Fix unaligned access exception

* Latest gptp snapshot

* Clean prop delay workaround for lowrisc_eth.

* Remove prop_delay workaround.
Timestamp issue fixed in lowrisc eth driver.

Author: brudley

samples/net/gptp/prj.conf

@@ -44,7 +44,7 @@ CONFIG_NET_CONFIG_PEER_IPV4_ADDR="192.0.2.2"
 # gPTP settings
 CONFIG_NET_GPTP=y
 CONFIG_NET_GPTP_STATISTICS=y
-CONFIG_NET_GPTP_GM_CAPABLE=y
+CONFIG_NET_GPTP_GM_CAPABLE=n
 
 # How many traffic classes to enable
 CONFIG_NET_TC_TX_COUNT=6

subsys/net/l2/ethernet/gptp/gptp_data_set.h

@@ -277,7 +277,7 @@ struct gptp_default_ds {
 	bool gm_capable;
 
 	/** Number of ports of the time-aware system. */
-	uint8_t nb_ports;
+	uint32_t nb_ports;
 
 	/** Primary priority of the time-aware system. */
 	uint8_t priority1;

subsys/net/l2/ethernet/gptp/gptp_md.c

@@ -12,6 +12,10 @@ LOG_MODULE_DECLARE(net_gptp, CONFIG_NET_GPTP_LOG_LEVEL);
 #include "gptp_data_set.h"
 #include "gptp_private.h"
 
+#ifdef CONFIG_PTP_CLOCK_LOWRISC
+#include <posix/time.h>
+#endif
+
 static void gptp_md_sync_prepare(struct net_pkt *pkt,
 				 struct gptp_md_sync_info *sync_send,
 				 int port_number)
@@ -101,6 +105,10 @@ static int gptp_set_md_sync_receive(int port,
 	struct net_ptp_time *sync_ts;
 	double prop_delay_rated;
 	double delay_asymmetry_rated;
+#ifdef CONFIG_PTP_CLOCK_LOWRISC
+#define UPDATE_SECS        30
+        static int count = UPDATE_SECS;
+#endif
 
 	state = &GPTP_PORT_STATE(port)->sync_rcv;
 	if (!state->rcvd_sync_ptr || !state->rcvd_follow_up_ptr) {
@@ -129,6 +137,30 @@ static int gptp_set_md_sync_receive(int port,
 	sync_rcv->upstream_tx_time *= NSEC_PER_SEC;
 	sync_rcv->upstream_tx_time += sync_ts->nanosecond;
 
+#ifdef CONFIG_PTP_CLOCK_LOWRISC
+        if (++count >= UPDATE_SECS) {
+                struct timespec tp;
+                tp.tv_sec = sync_rcv->precise_orig_ts.second;
+                tp.tv_nsec = sync_rcv->precise_orig_ts.nanosecond;
+                clock_settime(CLOCK_REALTIME, &tp);
+
+                if (0) {
+                        struct tm tm, *tm_p = &tm;
+                        gmtime_r(&tp.tv_sec, tm_p);
+                        NET_WARN("seq %d sec:ns  %d:%d  %d-%02u-%02u " "%02u:%02u:%02u UTC",
+				ntohs(fup_hdr->sequence_id),
+			    sync_rcv->precise_orig_ts._sec.low, sync_rcv->precise_orig_ts.nanosecond,
+                            tm_p->tm_year + 1900,
+                            tm_p->tm_mon + 1,
+                            tm_p->tm_mday,
+                            tm_p->tm_hour,
+                            tm_p->tm_min,
+                            tm_p->tm_sec);
+                }
+                count = 0;
+        }
+#endif
+
 	prop_delay_rated = port_ds->neighbor_prop_delay;
 	prop_delay_rated /= port_ds->neighbor_rate_ratio;
 
@@ -402,6 +434,9 @@ static void gptp_md_pdelay_compute(int port)
 	 */
 	if ((port_ds->neighbor_prop_delay <=
 	     port_ds->neighbor_prop_delay_thresh)) {
+		if (port_ds->as_capable == false) {
+		    NET_WARN("AS capable: true");
+		}
 		port_ds->as_capable = true;
 	} else {
 		port_ds->as_capable = false;

subsys/net/l2/ethernet/gptp/gptp_messages.c

@@ -643,7 +643,9 @@ void gptp_handle_pdelay_req(int port, struct net_pkt *pkt)
 	GPTP_STATS_INC(port, rx_pdelay_req_count);
 
 	if (ts_cb_registered == true) {
+#ifndef CONFIG_PTP_CLOCK_LOWRISC
 		NET_WARN("Multiple pdelay requests");
+#endif
 
 		net_if_unregister_timestamp_cb(&pdelay_response_timestamp_cb);
 		net_pkt_unref(pdelay_response_timestamp_cb.pkt);

subsys/net/l2/ethernet/gptp/gptp_mi.c

@@ -746,6 +746,8 @@ static void gptp_update_local_port_clock(void)
 	struct net_ptp_time tm;
 	int key;
 
+	NET_WARN("Setting CLOCK!\n");
+
 	state = &GPTP_STATE()->clk_slave_sync;
 	global_ds = GPTP_GLOBAL_DS();
 	port = state->pss_rcv_ptr->local_port_number;
@@ -865,6 +867,8 @@ static void gptp_mi_clk_slave_sync_state_machine(void)
 			gptp_update_local_port_clock();
 #endif
 			gptp_call_phase_dis_cb();
+		} else  {
+		        NET_WARN("slave_sync: would set clock but !state->rcvd_pss");
 		}
 
 		break;
@@ -920,6 +924,7 @@ static inline void gptp_mi_setup_sync_send_time(void)
 	struct gptp_global_ds *global_ds;
 	uint64_t time_helper;
 
+	NET_WARN("Recogni should never be here");
 	state = &GPTP_STATE()->clk_master_sync_send;
 	global_ds = GPTP_GLOBAL_DS();
 
@@ -1721,6 +1726,7 @@ static int compute_best_vector(void)
 		global_ds->gm_priority.port_number = best_vector->port_number;
 	}
 
+        NET_WARN("compute_best_vector: %d", best_port);
 	return best_port;
 }
 
@@ -1864,6 +1870,8 @@ static void gptp_updt_roles_tree(void)
 		(gm_prio->root_system_id.grand_master_prio1 == 255U) ?
 		false : true;
 
+	NET_WARN("gptp_updt_roles_tree(): gm_present = %d, master_prio %d", global_ds->gm_present, gm_prio->root_system_id.grand_master_prio1);
+
 	/* Assign the port role for port 0. */
 	for (port = GPTP_PORT_START; port < GPTP_PORT_END; port++) {
 		if (global_ds->selected_role[port] == GPTP_PORT_SLAVE) {