OSDN Git Service

639417964db5c752bd9ee76d0f3509734621bcad
[immortalwrt/immortalwrt.git] / package / kernel / fast-classifier / src / nl_classifier_test.c
1 /*
2  * Copyright (c) 2016 The Linux Foundation. All rights reserved.
3  * Permission to use, copy, modify, and/or distribute this software for
4  * any purpose with or without fee is hereby granted, provided that the
5  * above copyright notice and this permission notice appear in all copies.
6  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
7  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
8  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
9  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
10  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
11  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT
12  * OF OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
13  */
14
15 #include <netlink/genl/genl.h>
16 #include <netlink/genl/ctrl.h>
17 #include <errno.h>
18 #include <stdio.h>
19 #include <signal.h>
20 #include <arpa/inet.h>
21
22 #define NL_CLASSIFIER_GENL_VERSION      1
23 #define NL_CLASSIFIER_GENL_FAMILY       "FC"
24 #define NL_CLASSIFIER_GENL_GROUP        "FC_MCGRP"
25 #define NL_CLASSIFIER_GENL_HDRSIZE      0
26
27 enum NL_CLASSIFIER_CMD {
28         NL_CLASSIFIER_CMD_UNSPEC,
29         NL_CLASSIFIER_CMD_ACCEL,
30         NL_CLASSIFIER_CMD_ACCEL_OK,
31         NL_CLASSIFIER_CMD_CONNECTION_CLOSED,
32         NL_CLASSIFIER_CMD_MAX,
33 };
34
35 enum NL_CLASSIFIER_ATTR {
36         NL_CLASSIFIER_ATTR_UNSPEC,
37         NL_CLASSIFIER_ATTR_TUPLE,
38         NL_CLASSIFIER_ATTR_MAX,
39 };
40
41 union nl_classifier_tuple_ip {
42         struct in_addr in;
43         struct in6_addr in6;
44 };
45
46 struct nl_classifier_tuple {
47         unsigned short af;
48         unsigned char proto;
49         union nl_classifier_tuple_ip src_ip;
50         union nl_classifier_tuple_ip dst_ip;
51         unsigned short sport;
52         unsigned short dport;
53         unsigned char smac[6];
54         unsigned char dmac[6];
55 };
56
57 struct nl_classifier_instance {
58         struct nl_sock *sock;
59         int family_id;
60         int group_id;
61         int stop;
62 };
63
64 struct nl_classifier_instance nl_cls_inst;
65
66 static struct nla_policy nl_classifier_genl_policy[(NL_CLASSIFIER_ATTR_MAX+1)] = {
67         [NL_CLASSIFIER_ATTR_TUPLE] = { .type = NLA_UNSPEC },
68 };
69
70 void nl_classifier_dump_nl_tuple(struct nl_classifier_tuple *tuple)
71 {
72         char ip_str[64];
73
74         printf("protocol = %s\n", (tuple->proto == IPPROTO_UDP) ? "udp" : ((tuple->proto == IPPROTO_TCP) ? "tcp" : "unknown"));
75         printf("source ip = %s\n", inet_ntop(tuple->af, &tuple->src_ip, ip_str, sizeof(ip_str)));
76         printf("destination ip = %s\n", inet_ntop(tuple->af, &tuple->dst_ip, ip_str, sizeof(ip_str)));
77         printf("source port = %d\n", ntohs(tuple->sport));
78         printf("destination port = %d\n", ntohs(tuple->dport));
79 }
80
81 int nl_classifier_msg_recv(struct nl_msg *msg, void *arg)
82 {
83         struct nlmsghdr *nlh = nlmsg_hdr(msg);
84         struct genlmsghdr *gnlh = nlmsg_data(nlh);
85         struct nlattr *attrs[(NL_CLASSIFIER_ATTR_MAX+1)];
86
87         genlmsg_parse(nlh, NL_CLASSIFIER_GENL_HDRSIZE, attrs, NL_CLASSIFIER_ATTR_MAX, nl_classifier_genl_policy);
88
89         switch (gnlh->cmd) {
90         case NL_CLASSIFIER_CMD_ACCEL_OK:
91                 printf("Acceleration successful:\n");
92                 nl_classifier_dump_nl_tuple(nla_data(attrs[NL_CLASSIFIER_ATTR_TUPLE]));
93                 return NL_OK;
94         case NL_CLASSIFIER_CMD_CONNECTION_CLOSED:
95                 printf("Connection is closed:\n");
96                 nl_classifier_dump_nl_tuple(nla_data(attrs[NL_CLASSIFIER_ATTR_TUPLE]));
97                 return NL_OK;
98         default:
99                 printf("nl classifier received unknow message %d\n", gnlh->cmd);
100         }
101
102         return NL_SKIP;
103 }
104
105 void nl_classifier_offload(struct nl_classifier_instance *inst,
106                            unsigned char proto, unsigned long *src_saddr,
107                            unsigned long *dst_saddr, unsigned short sport,
108                            unsigned short dport, int af)
109 {
110         struct nl_msg *msg;
111         int ret;
112         struct nl_classifier_tuple classifier_msg;
113
114         memset(&classifier_msg, 0, sizeof(classifier_msg));
115         classifier_msg.af = af;
116         classifier_msg.proto = proto;
117         memcpy(&classifier_msg.src_ip, src_saddr, (af == AF_INET ? 4 : 16));
118         memcpy(&classifier_msg.dst_ip, dst_saddr, (af == AF_INET ? 4 : 16));
119         classifier_msg.sport = sport;
120         classifier_msg.dport = dport;
121
122         msg = nlmsg_alloc();
123         if (!msg) {
124                 printf("Unable to allocate message\n");
125                 return;
126         }
127
128         genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, inst->family_id,
129                     NL_CLASSIFIER_GENL_HDRSIZE, NLM_F_REQUEST,
130                     NL_CLASSIFIER_CMD_ACCEL, NL_CLASSIFIER_GENL_VERSION);
131         nla_put(msg, NL_CLASSIFIER_ATTR_TUPLE, sizeof(classifier_msg), &classifier_msg);
132
133         ret = nl_send_auto(inst->sock, msg);
134         if (ret < 0) {
135                 printf("send netlink message failed.\n");
136                 nlmsg_free(msg);
137                 return;
138         }
139
140         nlmsg_free(msg);
141         printf("nl classifier offload connection successful\n");
142 }
143
144 int nl_classifier_init(struct nl_classifier_instance *inst)
145 {
146         int ret;
147
148         inst->sock = nl_socket_alloc();
149         if (!inst->sock) {
150                 printf("Unable to allocation socket.\n");
151                 return -1;
152         }
153         genl_connect(inst->sock);
154
155         inst->family_id = genl_ctrl_resolve(inst->sock, NL_CLASSIFIER_GENL_FAMILY);
156         if (inst->family_id < 0) {
157                 printf("Unable to resolve family %s\n", NL_CLASSIFIER_GENL_FAMILY);
158                 goto init_failed;
159         }
160
161         inst->group_id = genl_ctrl_resolve_grp(inst->sock, NL_CLASSIFIER_GENL_FAMILY, NL_CLASSIFIER_GENL_GROUP);
162         if (inst->group_id < 0) {
163                 printf("Unable to resolve mcast group %s\n", NL_CLASSIFIER_GENL_GROUP);
164                 goto init_failed;
165         }
166
167         ret = nl_socket_add_membership(inst->sock, inst->group_id);
168         if (ret < 0) {
169                 printf("Unable to add membership\n");
170                 goto init_failed;
171         }
172
173         nl_socket_disable_seq_check(inst->sock);
174         nl_socket_modify_cb(inst->sock, NL_CB_VALID, NL_CB_CUSTOM, nl_classifier_msg_recv, NULL);
175
176         printf("nl classifier init successful\n");
177         return 0;
178
179 init_failed:
180         if (inst->sock) {
181                 nl_close(inst->sock);
182                 nl_socket_free(inst->sock);
183                 inst->sock = NULL;
184         }
185         return -1;
186 }
187
188 void nl_classifier_exit(struct nl_classifier_instance *inst)
189 {
190         if (inst->sock) {
191                 nl_close(inst->sock);
192                 nl_socket_free(inst->sock);
193                 inst->sock = NULL;
194         }
195         printf("nl classifier exit successful\n");
196 }
197
198 int nl_classifier_parse_arg(int argc, char *argv[], unsigned char *proto, unsigned long *src_saddr,
199                             unsigned long *dst_saddr, unsigned short *sport, unsigned short *dport, int *af)
200 {
201         int ret;
202         unsigned short port;
203
204         if (argc < 7) {
205                 printf("help: nl_classifier <v4|v6> <udp|tcp> <source ip> <destination ip> <source port> <destination port>\n");
206                 return -1;
207         }
208
209         if (0 == strncmp(argv[1], "v4", 2)) {
210                 *af = AF_INET;
211         } else if (0 == strncmp(argv[1], "v6", 2)) {
212                 *af = AF_INET6;
213         } else {
214                 printf("Address family is not supported");
215                 return -1;
216         }
217
218         if (0 == strncmp(argv[2], "udp", 3)) {
219                 *proto = IPPROTO_UDP;
220         } else if (0 == strncmp(argv[2], "tcp", 3)) {
221                 *proto = IPPROTO_TCP;
222         } else {
223                 printf("Protocol is not supported");
224                 return -1;
225         }
226
227         ret = inet_pton(*af, argv[3], src_saddr);
228         if (ret <= 0) {
229                 printf("source ip has wrong format\n");
230                 return -1;
231         }
232
233         ret = inet_pton(*af, argv[4], dst_saddr);
234         if (ret <= 0) {
235                 printf("destination ip has wrong format\n");
236                 return -1;
237         }
238
239         port = strtol(argv[5], NULL, 0);
240         *sport = htons(port);
241         port = strtol(argv[6], NULL, 0);
242         *dport = htons(port);
243
244         printf("nl classifier parse arguments successful\n");
245         return 0;
246 }
247
248 int main(int argc, char *argv[])
249 {
250         struct nl_classifier_instance *inst = &nl_cls_inst;
251         unsigned char proto;
252         unsigned long src_addr[4];
253         unsigned long dst_addr[4];
254         unsigned short sport;
255         unsigned short dport;
256         int af;
257         int ret;
258
259         ret = nl_classifier_parse_arg(argc, argv, &proto, src_addr, dst_addr, &sport, &dport, &af);
260         if (ret < 0) {
261                 printf("Failed to parse arguments\n");
262                 return ret;
263         }
264
265         ret = nl_classifier_init(inst);
266         if (ret < 0) {
267                 printf("Unable to init generic netlink\n");
268                 return ret;
269         }
270
271         nl_classifier_offload(inst, proto, src_addr, dst_addr, sport, dport, af);
272
273         /* main loop to listen on message */
274         while (!inst->stop) {
275                 nl_recvmsgs_default(inst->sock);
276         }
277
278         nl_classifier_exit(inst);
279
280         return 0;
281 }