1 /*
2 * Copyright (c) 2018 Intel Corporation.
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/logging/log.h>
8 LOG_MODULE_REGISTER(net_gptp_sample, LOG_LEVEL_DBG);
9
10 #include <zephyr/kernel.h>
11 #include <errno.h>
12
13 #include <zephyr/net/net_core.h>
14 #include <zephyr/net/net_l2.h>
15 #include <zephyr/net/net_if.h>
16 #include <zephyr/net/ethernet.h>
17 #include <zephyr/net/gptp.h>
18
19 extern void init_testing(void);
20
21 static struct gptp_phase_dis_cb phase_dis;
22
23 #if defined(CONFIG_NET_GPTP_VLAN)
24 /* User data for the interface callback */
25 struct ud {
26 struct net_if *first;
27 struct net_if *second;
28 struct net_if *third;
29 };
30
iface_cb(struct net_if * iface,void * user_data)31 static void iface_cb(struct net_if *iface, void *user_data)
32 {
33 struct ud *ud = user_data;
34
35 if (net_if_l2(iface) != &NET_L2_GET_NAME(ETHERNET)) {
36 return;
37 }
38
39 if (!ud->first) {
40 ud->first = iface;
41 return;
42 }
43
44 if (!ud->second) {
45 ud->second = iface;
46 return;
47 }
48
49 if (!ud->third) {
50 ud->third = iface;
51 return;
52 }
53 }
54
setup_iface(struct net_if * iface,const char * ipv6_addr,const char * ipv4_addr,uint16_t vlan_tag)55 static int setup_iface(struct net_if *iface, const char *ipv6_addr,
56 const char *ipv4_addr, uint16_t vlan_tag)
57 {
58 struct net_if_addr *ifaddr;
59 struct in_addr addr4;
60 struct in6_addr addr6;
61 int ret;
62
63 ret = net_eth_vlan_enable(iface, vlan_tag);
64 if (ret < 0) {
65 LOG_ERR("Cannot enable VLAN for tag %d (%d)", vlan_tag, ret);
66 }
67
68 if (net_addr_pton(AF_INET6, ipv6_addr, &addr6)) {
69 LOG_ERR("Invalid address: %s", ipv6_addr);
70 return -EINVAL;
71 }
72
73 ifaddr = net_if_ipv6_addr_add(iface, &addr6, NET_ADDR_MANUAL, 0);
74 if (!ifaddr) {
75 LOG_ERR("Cannot add %s to interface %p", ipv6_addr, iface);
76 return -EINVAL;
77 }
78
79 if (net_addr_pton(AF_INET, ipv4_addr, &addr4)) {
80 LOG_ERR("Invalid address: %s", ipv4_addr);
81 return -EINVAL;
82 }
83
84 ifaddr = net_if_ipv4_addr_add(iface, &addr4, NET_ADDR_MANUAL, 0);
85 if (!ifaddr) {
86 LOG_ERR("Cannot add %s to interface %p", ipv4_addr, iface);
87 return -EINVAL;
88 }
89
90 LOG_DBG("Interface %p VLAN tag %d setup done.", iface, vlan_tag);
91
92 return 0;
93 }
94
init_vlan(void)95 static int init_vlan(void)
96 {
97 struct ud ud;
98 int ret;
99
100 (void)memset(&ud, 0, sizeof(ud));
101
102 net_if_foreach(iface_cb, &ud);
103
104 /* This sample has two VLANs. For the second one we need to manually
105 * create IP address for this test. But first the VLAN needs to be
106 * added to the interface so that IPv6 DAD can work properly.
107 */
108 ret = setup_iface(ud.second,
109 CONFIG_NET_SAMPLE_IFACE2_MY_IPV6_ADDR,
110 CONFIG_NET_SAMPLE_IFACE2_MY_IPV4_ADDR,
111 CONFIG_NET_SAMPLE_IFACE2_VLAN_TAG);
112 if (ret < 0) {
113 return ret;
114 }
115
116 ret = setup_iface(ud.third,
117 CONFIG_NET_SAMPLE_IFACE3_MY_IPV6_ADDR,
118 CONFIG_NET_SAMPLE_IFACE3_MY_IPV4_ADDR,
119 CONFIG_NET_SAMPLE_IFACE3_VLAN_TAG);
120 if (ret < 0) {
121 return ret;
122 }
123
124 return 0;
125 }
126 #endif /* CONFIG_NET_GPTP_VLAN */
127
gptp_phase_dis_cb(uint8_t * gm_identity,uint16_t * time_base,struct gptp_scaled_ns * last_gm_ph_change,double * last_gm_freq_change)128 static void gptp_phase_dis_cb(uint8_t *gm_identity,
129 uint16_t *time_base,
130 struct gptp_scaled_ns *last_gm_ph_change,
131 double *last_gm_freq_change)
132 {
133 char output[sizeof("xx:xx:xx:xx:xx:xx:xx:xx")];
134 static uint8_t id[8];
135
136 if (memcmp(id, gm_identity, sizeof(id))) {
137 memcpy(id, gm_identity, sizeof(id));
138
139 LOG_DBG("GM %s last phase %d.%" PRId64 "",
140 gptp_sprint_clock_id(gm_identity, output, sizeof(output)),
141 last_gm_ph_change->high,
142 last_gm_ph_change->low);
143 }
144 }
145
init_app(void)146 static int init_app(void)
147 {
148 #if defined(CONFIG_NET_GPTP_VLAN)
149 if (init_vlan() < 0) {
150 LOG_ERR("Cannot setup VLAN");
151 }
152 #endif
153
154 gptp_register_phase_dis_cb(&phase_dis, gptp_phase_dis_cb);
155
156 return 0;
157 }
158
main(void)159 int main(void)
160 {
161 init_app();
162
163 init_testing();
164 return 0;
165 }
166
