├── .gitignore ├── LICENSE ├── Makefile ├── README.md ├── gtp5g.c └── gtp5g.h /.gitignore: -------------------------------------------------------------------------------- 1 | # Compile output 2 | *.o 3 | *.ko 4 | *.cmd 5 | *.mod 6 | *.mod.c 7 | Module.symvers 8 | modules.order -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | 294 | Copyright (C) 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | , 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | PWD := $(shell pwd) 2 | KVERSION := $(shell uname -r) 3 | INCLUDE_DIR = /usr/src/linux-headers-$(KVERSION)/ 4 | 5 | CONFIG_MODULE_SIG=n 6 | MODULE_NAME = gtp5g 7 | 8 | MY_CFLAGS += -g -DDEBUG 9 | EXTRA_CFLAGS += -Wno-misleading-indentation -Wuninitialized 10 | CC += ${MY_CFLAGS} 11 | 12 | obj-m := $(MODULE_NAME).o 13 | 14 | all: 15 | make -C $(INCLUDE_DIR) M=$(PWD) modules 16 | clean: 17 | make -C $(INCLUDE_DIR) M=$(PWD) clean 18 | 19 | install: 20 | modprobe udp_tunnel 21 | cp $(MODULE_NAME).ko /lib/modules/`uname -r`/kernel/drivers/net 22 | depmod -a 23 | modprobe $(MODULE_NAME) 24 | echo "gtp5g" >> /etc/modules 25 | 26 | uninstall: 27 | rmmod $(MODULE_NAME) 28 | rm -f /lib/modules/`uname -r`/kernel/drivers/net/$(MODULE_NAME).ko 29 | depmod -a 30 | sed -zi "s/gtp5g\n//g" /etc/modules 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # gtp5g - 5G compatible GTP kernel module 2 | gtp5g is a customized Linux kernel module gtp5g to handle packet by PFCP IEs such as PDR and FAR. 3 | For detailed information, please reference to 3GPP specification TS 29.281 and TS 29.244. 4 | 5 | ## Note:1 6 | The latest implementation of gtp5g and libgtp5gnl is in 7 | 8 | https://github.com/free5gc/gtp5g 9 | 10 | https://github.com/free5gc/libgtp5gnl 11 | 12 | 13 | ## Notice 14 | Due to the evolution of Linux kernel, this module would not work with every kernel version. 15 | Please run this module with kernel version `5.0.0-23-generic` or upper than `5.4` (Ubuntu 20.04). 16 | 17 | Please check the [libgtp5gnl](https://github.com/free5gc/libgtp5gnl) version is the same as gtp5g, 18 | because the type translating between libgtp5gnl and gtp5g had been changed. 19 | 20 | ## Usage 21 | ### Compile 22 | ``` 23 | make clean && make 24 | ``` 25 | 26 | ### Install kernel module 27 | Install the module to the system and load automatically at boot 28 | ``` 29 | sudo make install 30 | ``` 31 | 32 | ### Remove kernel module 33 | Remove the kernel module from the system 34 | ``` 35 | sudo make uninstall 36 | ``` 37 | ### Create a gtp5g interface and update Rules 38 | The gtp5g interface will be created by using libgtp5gnl scripts 39 | 1) Checkout the latest or compatible source of libgtp5gnl 40 | 2) cd libgtp5gnl 41 | 3) Create an interface and update rules 42 | + sudo ./run.sh UPF_PDR_FAR_QER 43 | 4) Troubleshoot 44 | + dmesg 45 | + echo #interfaceName #ID > /proc/gtp5g/pdr 46 | + cat /proc/gtp5g/pdr 47 | 5) Delete an interface 48 | + sudo ./run.sh Clean 49 | + Note: It will delete list of rules and interface 50 | 51 | ## Change Log 52 | ### v0.2.0 53 | + Changed to host type between gtp5g and libgtp5gnl 54 | + Supported forwarding policy feature, but need to set ip rule and ip route with Linux mark by self 55 | + Fixed bugs about "any" and "assigned" in SDF filter 56 | ### v0.1.0 57 | + First version 58 | + Supported packet matching about UE IP, TEID and SDF filter 59 | + Auto process packet by PDR/FAR 60 | -------------------------------------------------------------------------------- /gtp5g.c: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: GPL-2.0-or-later */ 2 | /* GTP5G according to 3GPP TS 29.281 / 3GPP TS 29.244 3 | * 4 | * Author: Muthuraman Elangovan 5 | * Yao-Wen Chang 6 | * Chi Chang 7 | */ 8 | 9 | #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "gtp5g.h" 37 | 38 | #define DRV_VERSION "1.0.2" 39 | 40 | int dbg_trace_lvl = 1; 41 | 42 | #define DBG(level, dev, fmt, args...) do { \ 43 | if (level <= dbg_trace_lvl) { \ 44 | if (dev) \ 45 | printk_ratelimited("%s:[gtp5g] %s: "fmt, netdev_name(dev), __func__, ##args); \ 46 | else \ 47 | printk_ratelimited("[gtp5g] %s: " fmt, __func__, ##args); \ 48 | } \ 49 | } while(0) 50 | 51 | #define GTP5G_LOG(dev, fmt, args...) DBG(0, dev, fmt, ##args) 52 | #define GTP5G_ERR(dev, fmt, args...) DBG(1, dev, fmt, ##args) 53 | #define GTP5G_WAR(dev, fmt, args...) DBG(2, dev, fmt, ##args) 54 | #define GTP5G_INF(dev, fmt, args...) DBG(3, dev, fmt, ##args) 55 | #define GTP5G_TRC(dev, fmt, args...) DBG(4, dev, fmt, ##args) 56 | 57 | struct local_f_teid { 58 | u32 teid; // i_teid 59 | struct in_addr gtpu_addr_ipv4; // self upf ip 60 | }; 61 | 62 | struct ip_filter_rule { 63 | uint8_t action; // permit only 64 | uint8_t direction; // in/out 65 | uint8_t proto; // number or "ip" which is not used for matching 66 | struct in_addr src, smask; // ip addr or "any" -> 0.0.0.0 67 | struct in_addr dest, dmask; // ip addr or "any" -> 0.0.0.0 68 | int sport_num; // Conut for sport 69 | struct range *sport; // one value, range or not existed -> [0, 0] 70 | int dport_num; // Counter for dport 71 | struct range *dport; // one value, range or not existed -> [0, 0] 72 | }; 73 | 74 | struct gtp5g_qer { 75 | struct hlist_node hlist_id; 76 | 77 | u32 id; /* 8.2.75 QER_ID */ 78 | uint8_t ul_dl_gate; /* 8.2.7 Gate Status */ 79 | struct { 80 | uint32_t ul_high; 81 | uint8_t ul_low; 82 | uint32_t dl_high; 83 | uint8_t dl_low; 84 | } mbr; /* 8.2.8 MBR */ 85 | struct { 86 | uint32_t ul_high; 87 | uint8_t ul_low; 88 | uint32_t dl_high; 89 | uint8_t dl_low; 90 | } gbr; /* 8.2.9 GBR */ 91 | uint32_t qer_corr_id; /* 8.2.10 QER Correlation ID */ 92 | uint8_t rqi; /* 8.2.88 RQI */ 93 | uint8_t qfi; /* 8.2.89 QFI */ 94 | 95 | /* 8.2.115 Averaging Window (Optional) */ 96 | 97 | uint8_t ppi; /* 8.2.116 Paging Policy Indicator */ 98 | 99 | /* 8.2.139 Packet Rate Status */ 100 | 101 | /* Rate Control Status Reporting */ 102 | uint8_t rcsr; /* 8.2.174 QER Control Indications */ 103 | 104 | struct net_device *dev; 105 | struct rcu_head rcu_head; 106 | }; 107 | 108 | struct sdf_filter { 109 | struct ip_filter_rule *rule; 110 | uint16_t *tos_traffic_class; 111 | u32 *security_param_idx; 112 | u32 *flow_label; // exactly 3 Octets 113 | u32 *bi_id; 114 | }; 115 | 116 | struct gtp5g_pdi { 117 | //u8 src_iface; // 0: Access, 1: Core, 2: SGi-LAN/N6-LAN, 3: CP-function 118 | struct in_addr *ue_addr_ipv4; 119 | //char *network_instance 120 | struct local_f_teid *f_teid; 121 | struct sdf_filter *sdf; 122 | }; 123 | 124 | struct outer_header_creation { 125 | u16 description; 126 | u32 teid; // o_teid 127 | struct in_addr peer_addr_ipv4; 128 | u16 port; 129 | }; 130 | 131 | struct forwarding_policy { 132 | int len; 133 | char identifier[0xff + 1]; 134 | 135 | /* Exact value to handle forwarding policy */ 136 | u32 mark; 137 | }; 138 | 139 | struct forwarding_parameter { 140 | //uint8_t dest_int; 141 | //char *network_instance; 142 | 143 | struct outer_header_creation *hdr_creation; 144 | struct forwarding_policy *fwd_policy; 145 | }; 146 | 147 | struct gtp5g_far { 148 | struct hlist_node hlist_id; 149 | 150 | u32 id; 151 | 152 | //u8 dest_iface; 153 | u8 action; // apply action 154 | 155 | struct forwarding_parameter *fwd_param; 156 | 157 | struct net_device *dev; 158 | struct rcu_head rcu_head; 159 | }; 160 | 161 | struct gtp5g_pdr { 162 | struct hlist_node hlist_id; 163 | struct hlist_node hlist_i_teid; 164 | struct hlist_node hlist_addr; 165 | struct hlist_node hlist_related_far; 166 | struct hlist_node hlist_related_qer; 167 | 168 | u16 id; 169 | u32 precedence; 170 | u8 *outer_header_removal; 171 | 172 | struct gtp5g_pdi *pdi; 173 | 174 | u32 *far_id; 175 | struct gtp5g_far *far; 176 | 177 | u32 *qer_id; 178 | struct gtp5g_qer *qer; 179 | 180 | // AF_UNIX socket for buffer 181 | struct sockaddr_un addr_unix; 182 | struct socket *sock_for_buf; 183 | 184 | u16 af; 185 | struct in_addr role_addr_ipv4; 186 | struct sock *sk; 187 | struct net_device *dev; 188 | struct rcu_head rcu_head; 189 | }; 190 | 191 | /* One instance of the GTP device. */ 192 | struct gtp5g_dev { 193 | struct list_head list; 194 | 195 | struct sock *sk1u; 196 | 197 | struct net_device *dev; 198 | 199 | unsigned int role; 200 | 201 | unsigned int hash_size; 202 | struct hlist_head *pdr_id_hash; 203 | struct hlist_head *far_id_hash; 204 | struct hlist_head *qer_id_hash; 205 | 206 | struct hlist_head *i_teid_hash; // Used for GTP-U packet detect 207 | struct hlist_head *addr_hash; // Used for IPv4 packet detect 208 | 209 | /* IEs list related to PDR */ 210 | struct hlist_head *related_far_hash; // PDR list waiting the FAR to handle 211 | struct hlist_head *related_qer_hash; // PDR list waiting the QER to handle 212 | 213 | /* Used by proc interface */ 214 | struct list_head proc_list; 215 | }; 216 | 217 | struct gtp5g_pktinfo { 218 | struct sock *sk; 219 | struct iphdr *iph; 220 | struct flowi4 fl4; 221 | struct rtable *rt; 222 | struct outer_header_creation *hdr_creation; 223 | struct gtp5g_qer *qer; 224 | struct net_device *dev; 225 | __be16 gtph_port; 226 | }; 227 | 228 | struct gtp5g_emark_pktinfo { 229 | u32 teid; 230 | u32 peer_addr; 231 | u32 local_addr; 232 | u32 role_addr; 233 | 234 | struct sock *sk; 235 | struct flowi4 fl4; 236 | struct rtable *rt; 237 | struct net_device *dev; 238 | __be16 gtph_port; 239 | }; 240 | 241 | 242 | static unsigned int gtp5g_net_id __read_mostly; 243 | 244 | struct gtp5g_net { 245 | struct list_head gtp5g_dev_list; 246 | }; 247 | 248 | struct list_head proc_gtp5g_dev; 249 | struct proc_gtp5g_pdr { 250 | u16 id; 251 | u32 precedence; 252 | u8 ohr; 253 | u32 role_addr4; 254 | 255 | u32 pdi_ue_addr4; 256 | u32 pdi_fteid; 257 | u32 pdi_gtpu_addr4; 258 | 259 | u32 far_id; 260 | u32 qer_id; 261 | }; 262 | 263 | static struct gtp5g_qer *gtp5g_find_qer(struct net *net, struct nlattr *nla[]); 264 | static struct gtp5g_qer *qer_find_by_id(struct gtp5g_dev *gtp, u32 id); 265 | static void qer_context_delete(struct gtp5g_qer *qer); 266 | 267 | /* Function unix_sock_{...} are used to handle buffering */ 268 | // Send PDR ID, FAR action and buffered packet to user space 269 | static int unix_sock_send(struct gtp5g_pdr *pdr, void *buf, u32 len) 270 | { 271 | struct msghdr msg; 272 | struct iovec iov[2]; 273 | mm_segment_t oldfs; 274 | 275 | int msg_iovlen = sizeof(iov) / sizeof(struct iovec); 276 | int total_iov_len = 0; 277 | int i, rt; 278 | u16 self_hdr[2] = {pdr->id, pdr->far->action}; 279 | 280 | if (!pdr->sock_for_buf) { 281 | GTP5G_ERR(NULL, "Failed Socket buffer is NULL\n"); 282 | return -EINVAL; 283 | } 284 | 285 | memset(&msg, 0, sizeof(msg)); 286 | memset(iov, 0, sizeof(iov)); 287 | 288 | iov[0].iov_base = self_hdr; 289 | iov[0].iov_len = sizeof(self_hdr); 290 | iov[1].iov_base = buf; 291 | iov[1].iov_len = len; 292 | 293 | for (i = 0; i < msg_iovlen; i++) 294 | total_iov_len += iov[i].iov_len; 295 | 296 | msg.msg_name = 0; 297 | msg.msg_namelen = 0; 298 | iov_iter_init(&msg.msg_iter, WRITE, iov, msg_iovlen, total_iov_len); 299 | msg.msg_control = NULL; 300 | msg.msg_controllen = 0; 301 | msg.msg_flags = 0; 302 | 303 | #if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 10, 0) 304 | oldfs = force_uaccess_begin(); 305 | #else 306 | oldfs = get_fs(); 307 | set_fs(KERNEL_DS); 308 | #endif 309 | 310 | rt = sock_sendmsg(pdr->sock_for_buf, &msg); 311 | 312 | #if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 10, 0) 313 | force_uaccess_end(oldfs); 314 | #else 315 | set_fs(oldfs); 316 | #endif 317 | 318 | 319 | return rt; 320 | } 321 | 322 | // Delete the AF_UNIX client 323 | static void unix_sock_client_delete(struct gtp5g_pdr *pdr) 324 | { 325 | if (pdr->sock_for_buf) 326 | sock_release(pdr->sock_for_buf); 327 | 328 | pdr->sock_for_buf = NULL; 329 | } 330 | 331 | // Create a AF_UNIX client by specific name sent from user space 332 | static int unix_sock_client_new(struct gtp5g_pdr *pdr) 333 | { 334 | int rt; 335 | struct socket **psock = &pdr->sock_for_buf; 336 | struct sockaddr_un *addr = &pdr->addr_unix; 337 | 338 | if (!strlen(addr->sun_path)) { 339 | GTP5G_ERR(NULL, "Failed sun_path length is 0\n"); 340 | return -EINVAL; 341 | } 342 | 343 | rt = sock_create(AF_UNIX, SOCK_DGRAM, 0, psock); 344 | if (rt) { 345 | GTP5G_ERR(NULL, "Failed to create dgram socket\n"); 346 | return rt; 347 | } 348 | 349 | rt = (*psock)->ops->connect(*psock, (struct sockaddr *) addr, 350 | sizeof(addr->sun_family) + strlen(addr->sun_path), 0); 351 | if (rt) { 352 | unix_sock_client_delete(pdr); 353 | GTP5G_ERR(NULL, "Failed to connect the socket\n"); 354 | return rt; 355 | } 356 | 357 | return 0; 358 | } 359 | 360 | // Handle PDR/FAR changed and affect buffering 361 | static int unix_sock_client_update(struct gtp5g_pdr *pdr) 362 | { 363 | struct gtp5g_far *far = pdr->far; 364 | 365 | unix_sock_client_delete(pdr); 366 | 367 | if (far && (far->action & FAR_ACTION_BUFF)) 368 | return unix_sock_client_new(pdr); 369 | 370 | return 0; 371 | } 372 | 373 | static u32 gtp5g_h_initval; 374 | 375 | static inline u32 u32_hashfn(u32 val) 376 | { 377 | return jhash_1word(val, gtp5g_h_initval); 378 | } 379 | 380 | static inline u32 ipv4_hashfn(__be32 ip) 381 | { 382 | return jhash_1word((__force u32)ip, gtp5g_h_initval); 383 | } 384 | 385 | static struct gtp5g_far *far_find_by_id(struct gtp5g_dev *gtp, u32 id) 386 | { 387 | struct hlist_head *head; 388 | struct gtp5g_far *far; 389 | 390 | head = >p->far_id_hash[u32_hashfn(id) % gtp->hash_size]; 391 | 392 | hlist_for_each_entry_rcu(far, head, hlist_id) { 393 | if (far->id == id) 394 | return far; 395 | } 396 | 397 | return NULL; 398 | } 399 | 400 | static int far_fill(struct gtp5g_far *far, struct gtp5g_dev *gtp, struct genl_info *info, 401 | u8 *flag, struct gtp5g_emark_pktinfo *epkt_info) 402 | { 403 | struct nlattr *fwd_param_attrs[GTP5G_FORWARDING_PARAMETER_ATTR_MAX + 1]; 404 | struct nlattr *hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_ATTR_MAX + 1]; 405 | struct outer_header_creation *hdr_creation; 406 | struct forwarding_policy *fwd_policy; 407 | 408 | // Update related PDR for buffering 409 | struct gtp5g_pdr *pdr; 410 | struct hlist_head *head; 411 | 412 | if (!far) { 413 | GTP5G_ERR(NULL, "Far is null\n"); 414 | return -EINVAL; 415 | } 416 | 417 | far->id = nla_get_u32(info->attrs[GTP5G_FAR_ID]); 418 | 419 | if (info->attrs[GTP5G_FAR_APPLY_ACTION]) { 420 | far->action = nla_get_u8(info->attrs[GTP5G_FAR_APPLY_ACTION]); 421 | } 422 | 423 | if (info->attrs[GTP5G_FAR_FORWARDING_PARAMETER] && 424 | !nla_parse_nested(fwd_param_attrs, 425 | GTP5G_FORWARDING_PARAMETER_ATTR_MAX, 426 | info->attrs[GTP5G_FAR_FORWARDING_PARAMETER], 427 | NULL, 428 | NULL)) { 429 | if (!far->fwd_param) { 430 | far->fwd_param = kzalloc(sizeof(*far->fwd_param), GFP_ATOMIC); 431 | if (!far->fwd_param) { 432 | GTP5G_ERR(NULL, "Failed to allocate FAR fwd param\n"); 433 | return -ENOMEM; 434 | } 435 | } 436 | 437 | if (fwd_param_attrs[GTP5G_FORWARDING_PARAMETER_OUTER_HEADER_CREATION] && 438 | !nla_parse_nested(hdr_creation_attrs, 439 | GTP5G_OUTER_HEADER_CREATION_ATTR_MAX, 440 | fwd_param_attrs[GTP5G_FORWARDING_PARAMETER_OUTER_HEADER_CREATION], 441 | NULL, 442 | NULL)) { 443 | 444 | if (!hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_DESCRIPTION] || 445 | !hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_O_TEID] || 446 | !hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_PEER_ADDR_IPV4] || 447 | !hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_PORT]) { 448 | GTP5G_ERR(NULL, "Header createion attributes error\n"); 449 | return -EINVAL; 450 | } 451 | 452 | if (!far->fwd_param->hdr_creation) { 453 | far->fwd_param->hdr_creation = kzalloc(sizeof(*far->fwd_param->hdr_creation), 454 | GFP_ATOMIC); 455 | if (!far->fwd_param->hdr_creation) { 456 | GTP5G_ERR(NULL, "Failed to allocate FAR fwd Hdr creation\n"); 457 | return -ENOMEM; 458 | } 459 | hdr_creation = far->fwd_param->hdr_creation; 460 | hdr_creation->description = nla_get_u16(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_DESCRIPTION]); 461 | hdr_creation->teid = htonl(nla_get_u32(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_O_TEID])); 462 | hdr_creation->peer_addr_ipv4.s_addr = nla_get_be32(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_PEER_ADDR_IPV4]); 463 | hdr_creation->port = htons(nla_get_u16(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_PORT])); 464 | } else { 465 | u32 old_teid, old_peer_addr; 466 | u16 old_port; 467 | 468 | hdr_creation = far->fwd_param->hdr_creation; 469 | old_teid = hdr_creation->teid; 470 | old_peer_addr = hdr_creation->peer_addr_ipv4.s_addr; 471 | old_port = hdr_creation->port; 472 | hdr_creation->description = nla_get_u16(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_DESCRIPTION]); 473 | hdr_creation->teid = htonl(nla_get_u32(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_O_TEID])); 474 | hdr_creation->peer_addr_ipv4.s_addr = nla_get_be32(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_PEER_ADDR_IPV4]); 475 | hdr_creation->port = htons(nla_get_u16(hdr_creation_attrs[GTP5G_OUTER_HEADER_CREATION_PORT])); 476 | /* For Downlink traffic from UPF to gNB 477 | * In some cases, 478 | * 1) SMF will send PFCP Msg filled with FAR's TEID and gNB N3 addr as 0 479 | * 2) Later time, SMF will send PFCP Msg filled with right value in 1) 480 | * 2.a) We should send the GTP-U EndMarker to gNB 481 | * 2.b) SHOULD not set the flag as 1 482 | * 3) Xn Handover in b/w gNB then 483 | * 3.a) SMF will send modification of PDR, FAR(TEID and GTP-U) 484 | * 3.b) SHOULD set the flag as 1 and send GTP-U Marker for old gNB 485 | * */ 486 | if ((flag != NULL && epkt_info != NULL)) { 487 | if (((old_teid & hdr_creation->teid) != 0 && ((old_peer_addr & hdr_creation->peer_addr_ipv4.s_addr) != 0)) && 488 | ((old_teid != hdr_creation->teid ) || (old_peer_addr != hdr_creation->peer_addr_ipv4.s_addr))) { 489 | *flag = 1; 490 | epkt_info->teid = old_teid; 491 | epkt_info->peer_addr = old_peer_addr; 492 | epkt_info->gtph_port = old_port; 493 | } 494 | } 495 | } 496 | } 497 | 498 | if (fwd_param_attrs[GTP5G_FORWARDING_PARAMETER_FORWARDING_POLICY]) { 499 | if (!far->fwd_param->fwd_policy) { 500 | far->fwd_param->fwd_policy = kzalloc(sizeof(*far->fwd_param->fwd_policy), 501 | GFP_ATOMIC); 502 | if (!far->fwd_param->fwd_policy) { 503 | GTP5G_ERR(NULL, "Failed to allocate FAR fwd policy\n"); 504 | return -ENOMEM; 505 | } 506 | } 507 | fwd_policy = far->fwd_param->fwd_policy; 508 | 509 | fwd_policy->len = nla_len(fwd_param_attrs[GTP5G_FORWARDING_PARAMETER_FORWARDING_POLICY]); 510 | if (fwd_policy->len >= sizeof(fwd_policy->identifier)) { 511 | GTP5G_ERR(NULL, "Failed FAR fwd policy length is bigger\n"); 512 | return -EINVAL; 513 | } 514 | strncpy(fwd_policy->identifier, 515 | nla_data(fwd_param_attrs[GTP5G_FORWARDING_PARAMETER_FORWARDING_POLICY]), 516 | fwd_policy->len); 517 | 518 | /* Exact value to handle forwarding policy */ 519 | if (!(fwd_policy->mark = simple_strtol(fwd_policy->identifier, NULL, 10))) { 520 | GTP5G_ERR(NULL, "Failed FAR fwd policy mark not set\n"); 521 | return -EINVAL; 522 | } 523 | } 524 | } 525 | 526 | /* Update PDRs which has not linked to this FAR */ 527 | head = >p->related_far_hash[u32_hashfn(far->id) % gtp->hash_size]; 528 | hlist_for_each_entry_rcu(pdr, head, hlist_related_far) { 529 | if (*pdr->far_id == far->id) { 530 | if (flag != NULL && *flag == 1) { 531 | epkt_info->role_addr = pdr->role_addr_ipv4.s_addr; 532 | epkt_info->sk = pdr->sk; 533 | GTP5G_INF(NULL, "Flag is set, role_addr %#x\n", epkt_info->role_addr); 534 | } 535 | pdr->far = far; 536 | if (unix_sock_client_update(pdr) < 0) 537 | GTP5G_ERR(NULL, "PDR(%u) update fail when FAR(%u) apply action is changed", 538 | pdr->id, far->id); 539 | } 540 | } 541 | 542 | return 0; 543 | } 544 | 545 | static struct gtp5g_pdr *pdr_find_by_id(struct gtp5g_dev *gtp, u16 id) 546 | { 547 | struct hlist_head *head; 548 | struct gtp5g_pdr *pdr; 549 | 550 | head = >p->pdr_id_hash[u32_hashfn(id) % gtp->hash_size]; 551 | hlist_for_each_entry_rcu(pdr, head, hlist_id) { 552 | if (pdr->id == id) 553 | return pdr; 554 | } 555 | 556 | return NULL; 557 | } 558 | 559 | static int ipv4_match(__be32 target_addr, __be32 ifa_addr, __be32 ifa_mask) { 560 | return !((target_addr ^ ifa_addr) & ifa_mask); 561 | } 562 | 563 | static int ports_match(struct range *match_list, int list_len, __be16 port) { 564 | int i; 565 | 566 | if (!list_len) 567 | return 1; 568 | 569 | for (i = 0; i < list_len; i++) { 570 | if (match_list[i].start <= port && match_list[i].end >= port) 571 | return 1; 572 | } 573 | return 0; 574 | } 575 | 576 | static int sdf_filter_match(struct sdf_filter *sdf, struct sk_buff *skb, unsigned int hdrlen, u8 direction) 577 | { 578 | struct iphdr *iph; 579 | struct ip_filter_rule *rule; 580 | 581 | const __be16 *pptr; 582 | __be16 _ports[2]; 583 | 584 | if (!sdf) { 585 | GTP5G_ERR(NULL, "SDF is null\n"); 586 | return 1; 587 | } 588 | 589 | if (!pskb_may_pull(skb, hdrlen + sizeof(struct iphdr))) { 590 | GTP5G_ERR(NULL, "skb pull fail\n"); 591 | goto mismatch; 592 | } 593 | 594 | iph = (struct iphdr *)(skb->data + hdrlen); 595 | 596 | if (sdf->rule) { 597 | rule = sdf->rule; 598 | if (rule->direction != direction) 599 | goto mismatch; 600 | 601 | if (rule->proto != 0xff && rule->proto != iph->protocol) 602 | goto mismatch; 603 | 604 | if (!ipv4_match(iph->saddr, rule->src.s_addr, rule->smask.s_addr)) 605 | goto mismatch; 606 | 607 | if (!ipv4_match(iph->daddr, rule->dest.s_addr, rule->dmask.s_addr)) 608 | goto mismatch; 609 | 610 | if (rule->sport_num + rule->dport_num > 0) { 611 | if (!(pptr = skb_header_pointer(skb, hdrlen + sizeof(struct iphdr), sizeof(_ports), _ports))) 612 | goto mismatch; 613 | 614 | if (!ports_match(rule->sport, rule->sport_num, ntohs(pptr[0]))) 615 | goto mismatch; 616 | 617 | if (!ports_match(rule->dport, rule->dport_num, ntohs(pptr[1]))) 618 | goto mismatch; 619 | } 620 | } 621 | 622 | if (sdf->tos_traffic_class) 623 | GTP5G_ERR(NULL, "ToS traffic class check does not implement yet\n"); 624 | 625 | if (sdf->security_param_idx) 626 | GTP5G_ERR(NULL, "Security parameter index check does not implement yet\n"); 627 | 628 | if (sdf->flow_label) 629 | GTP5G_ERR(NULL, "Flow label check does not implement yet\n"); 630 | 631 | if (sdf->bi_id) 632 | GTP5G_ERR(NULL, "SDF filter ID check does not implement yet\n"); 633 | 634 | return 1; 635 | 636 | mismatch: 637 | return 0; 638 | } 639 | 640 | static struct gtp5g_pdr *pdr_find_by_ipv4(struct gtp5g_dev *gtp, struct sk_buff *skb, 641 | unsigned int hdrlen, __be32 addr) 642 | { 643 | struct hlist_head *head; 644 | struct gtp5g_pdr *pdr; 645 | struct gtp5g_pdi *pdi; 646 | 647 | head = >p->addr_hash[ipv4_hashfn(addr) % gtp->hash_size]; 648 | 649 | hlist_for_each_entry_rcu(pdr, head, hlist_addr) { 650 | pdi = pdr->pdi; 651 | 652 | // TODO: Move the value we check into first level 653 | if (!(pdr->af == AF_INET && pdi->ue_addr_ipv4->s_addr == addr)) 654 | continue; 655 | 656 | if (pdi->sdf) 657 | if (!sdf_filter_match(pdi->sdf, skb, hdrlen, GTP5G_SDF_FILTER_OUT)) 658 | continue; 659 | 660 | return pdr; 661 | } 662 | 663 | return NULL; 664 | } 665 | 666 | static int pdr_fill(struct gtp5g_pdr *pdr, struct gtp5g_dev *gtp, struct genl_info *info) 667 | { 668 | struct nlattr *pdi_attrs[GTP5G_PDI_ATTR_MAX + 1]; 669 | struct nlattr *f_teid_attrs[GTP5G_F_TEID_ATTR_MAX + 1]; 670 | struct nlattr *sdf_attrs[GTP5G_SDF_FILTER_ATTR_MAX + 1]; 671 | struct nlattr *rule_attrs[GTP5G_FLOW_DESCRIPTION_ATTR_MAX + 1]; 672 | struct hlist_head *head; 673 | struct gtp5g_pdr *ppdr, *last_ppdr; 674 | struct gtp5g_pdi *pdi = NULL; 675 | struct local_f_teid *f_teid = NULL; 676 | struct sdf_filter *sdf; 677 | struct ip_filter_rule *rule; 678 | int i; 679 | char *str; 680 | 681 | if (!pdr) { 682 | GTP5G_ERR(NULL, "PDR is NULL\n"); 683 | return -EINVAL; 684 | } 685 | 686 | pdr->af = AF_INET; 687 | pdr->id = nla_get_u16(info->attrs[GTP5G_PDR_ID]); 688 | 689 | if (info->attrs[GTP5G_PDR_PRECEDENCE]) 690 | pdr->precedence = nla_get_u32(info->attrs[GTP5G_PDR_PRECEDENCE]); 691 | 692 | if (info->attrs[GTP5G_OUTER_HEADER_REMOVAL]) { 693 | if (!pdr->outer_header_removal) { 694 | pdr->outer_header_removal = kzalloc(sizeof(*pdr->outer_header_removal), GFP_ATOMIC); 695 | if (!pdr->outer_header_removal) { 696 | GTP5G_ERR(NULL, "Failed to allocate OHC\n"); 697 | return -ENOMEM; 698 | } 699 | } 700 | *pdr->outer_header_removal = nla_get_u8(info->attrs[GTP5G_OUTER_HEADER_REMOVAL]); 701 | } 702 | 703 | /* Not in 3GPP spec, just used for routing */ 704 | if (info->attrs[GTP5G_PDR_ROLE_ADDR_IPV4]) { 705 | pdr->role_addr_ipv4.s_addr = nla_get_u32(info->attrs[GTP5G_PDR_ROLE_ADDR_IPV4]); 706 | } 707 | 708 | /* Not in 3GPP spec, just used for buffering */ 709 | if (info->attrs[GTP5G_PDR_UNIX_SOCKET_PATH]) { 710 | str = nla_data(info->attrs[GTP5G_PDR_UNIX_SOCKET_PATH]); 711 | pdr->addr_unix.sun_family = AF_UNIX; 712 | strncpy(pdr->addr_unix.sun_path, str, nla_len(info->attrs[GTP5G_PDR_UNIX_SOCKET_PATH])); 713 | } 714 | 715 | /* FAR */ 716 | if (info->attrs[GTP5G_PDR_FAR_ID]) { 717 | if (!pdr->far_id) { 718 | pdr->far_id = kzalloc(sizeof(*pdr->far_id), GFP_ATOMIC); 719 | if (!pdr->far_id) { 720 | GTP5G_ERR(NULL, "Failed to allocate FAR ID\n"); 721 | return -ENOMEM; 722 | } 723 | } 724 | *pdr->far_id = nla_get_u32(info->attrs[GTP5G_PDR_FAR_ID]); 725 | 726 | if (!hlist_unhashed(&pdr->hlist_related_far)) 727 | hlist_del_rcu(&pdr->hlist_related_far); 728 | 729 | hlist_add_head_rcu(&pdr->hlist_related_far, 730 | >p->related_far_hash[u32_hashfn(*pdr->far_id) % gtp->hash_size]); 731 | pdr->far = far_find_by_id(gtp, *pdr->far_id); 732 | } else { 733 | GTP5G_ERR(NULL, "FAR ID not exist\n"); 734 | } 735 | 736 | /* QER */ 737 | if (info->attrs[GTP5G_PDR_QER_ID]) { 738 | if (!pdr->qer_id) { 739 | pdr->qer_id = kzalloc(sizeof(*pdr->qer_id), GFP_ATOMIC); 740 | if (!pdr->qer_id) { 741 | GTP5G_ERR(NULL, "Failed to allocate memory qer id\n"); 742 | return -ENOMEM; 743 | } 744 | } 745 | *pdr->qer_id = nla_get_u32(info->attrs[GTP5G_PDR_QER_ID]); 746 | 747 | if (!hlist_unhashed(&pdr->hlist_related_qer)) 748 | hlist_del_rcu(&pdr->hlist_related_qer); 749 | 750 | hlist_add_head_rcu(&pdr->hlist_related_qer, 751 | >p->related_qer_hash[u32_hashfn(*pdr->qer_id) % gtp->hash_size]); 752 | 753 | pdr->qer = qer_find_by_id(gtp, *pdr->qer_id); 754 | if (!pdr->qer) 755 | GTP5G_ERR(NULL, "Failed to find QER id(%u)\n", *pdr->qer_id); 756 | } 757 | 758 | if (unix_sock_client_update(pdr) < 0) { 759 | GTP5G_ERR(NULL, "PDR sock client update fail\n"); 760 | return -EINVAL; 761 | } 762 | 763 | /* Parse PDI in PDR */ 764 | if (info->attrs[GTP5G_PDR_PDI] && 765 | !nla_parse_nested(pdi_attrs, 766 | GTP5G_PDI_ATTR_MAX, 767 | info->attrs[GTP5G_PDR_PDI], 768 | NULL, 769 | NULL)) { 770 | if (!pdr->pdi) { 771 | pdr->pdi = kzalloc(sizeof(*pdr->pdi), GFP_ATOMIC); 772 | if (!pdr->pdi) { 773 | GTP5G_ERR(NULL, "Failed to allocate PDI\n"); 774 | return -ENOMEM; 775 | } 776 | } 777 | pdi = pdr->pdi; 778 | 779 | if (pdi_attrs[GTP5G_PDI_UE_ADDR_IPV4]) { 780 | if (!pdi->ue_addr_ipv4) { 781 | pdi->ue_addr_ipv4 = kzalloc(sizeof(*pdi->ue_addr_ipv4), GFP_ATOMIC); 782 | if (!pdi->ue_addr_ipv4) { 783 | GTP5G_ERR(NULL, "Failed to allocate UE IPv4 address\n"); 784 | return -ENOMEM; 785 | } 786 | } 787 | 788 | pdi->ue_addr_ipv4->s_addr = nla_get_be32(pdi_attrs[GTP5G_PDI_UE_ADDR_IPV4]); 789 | } 790 | 791 | /* Parse F-TEID in PDI */ 792 | if (pdi_attrs[GTP5G_PDI_F_TEID] && 793 | !nla_parse_nested(f_teid_attrs, 794 | GTP5G_F_TEID_ATTR_MAX, 795 | pdi_attrs[GTP5G_PDI_F_TEID], 796 | NULL, 797 | NULL)) { 798 | if (!f_teid_attrs[GTP5G_F_TEID_I_TEID] || 799 | !f_teid_attrs[GTP5G_F_TEID_GTPU_ADDR_IPV4]) { 800 | GTP5G_ERR(NULL, "TEID is not preset\n"); 801 | return -EINVAL; 802 | } 803 | 804 | if (!pdi->f_teid) { 805 | pdi->f_teid = kzalloc(sizeof(*pdi->f_teid), GFP_ATOMIC); 806 | if (!pdi->f_teid) { 807 | GTP5G_ERR(NULL, "Failed to allocate UE IPv4 address\n"); 808 | return -ENOMEM; 809 | } 810 | } 811 | f_teid = pdi->f_teid; 812 | f_teid->teid = htonl(nla_get_u32(f_teid_attrs[GTP5G_F_TEID_I_TEID])); 813 | f_teid->gtpu_addr_ipv4.s_addr = nla_get_be32(f_teid_attrs[GTP5G_F_TEID_GTPU_ADDR_IPV4]); 814 | } 815 | 816 | /* Parse SDF Filter in PDI */ 817 | if (pdi_attrs[GTP5G_PDI_SDF_FILTER] && 818 | !nla_parse_nested(sdf_attrs, 819 | GTP5G_SDF_FILTER_ATTR_MAX, 820 | pdi_attrs[GTP5G_PDI_SDF_FILTER], 821 | NULL, 822 | NULL)) { 823 | if (!pdi->sdf) { 824 | pdi->sdf = kzalloc(sizeof(*pdi->sdf), GFP_ATOMIC); 825 | if (!pdi->sdf) { 826 | GTP5G_ERR(NULL, "Failed to allocate SDF\n"); 827 | return -ENOMEM; 828 | } 829 | } 830 | sdf = pdi->sdf; 831 | 832 | if (sdf_attrs[GTP5G_SDF_FILTER_FLOW_DESCRIPTION] && 833 | !nla_parse_nested(rule_attrs, 834 | GTP5G_FLOW_DESCRIPTION_ATTR_MAX, 835 | sdf_attrs[GTP5G_SDF_FILTER_FLOW_DESCRIPTION], 836 | NULL, 837 | NULL)) { 838 | if (!rule_attrs[GTP5G_FLOW_DESCRIPTION_ACTION] || 839 | !rule_attrs[GTP5G_FLOW_DESCRIPTION_DIRECTION] || 840 | !rule_attrs[GTP5G_FLOW_DESCRIPTION_PROTOCOL] || 841 | !rule_attrs[GTP5G_FLOW_DESCRIPTION_SRC_IPV4] || 842 | !rule_attrs[GTP5G_FLOW_DESCRIPTION_DEST_IPV4]) 843 | return -EINVAL; 844 | 845 | if (!sdf->rule) { 846 | sdf->rule = kzalloc(sizeof(*sdf->rule), GFP_ATOMIC); 847 | if (!sdf->rule) { 848 | GTP5G_ERR(NULL, "Failed to allocate SDF's Rule\n"); 849 | return -ENOMEM; 850 | } 851 | } 852 | rule = sdf->rule; 853 | 854 | rule->action = nla_get_u8(rule_attrs[GTP5G_FLOW_DESCRIPTION_ACTION]); 855 | rule->direction = nla_get_u8(rule_attrs[GTP5G_FLOW_DESCRIPTION_DIRECTION]); 856 | rule->proto = nla_get_u8(rule_attrs[GTP5G_FLOW_DESCRIPTION_PROTOCOL]); 857 | rule->src.s_addr = nla_get_be32(rule_attrs[GTP5G_FLOW_DESCRIPTION_SRC_IPV4]); 858 | rule->dest.s_addr = nla_get_be32(rule_attrs[GTP5G_FLOW_DESCRIPTION_DEST_IPV4]); 859 | 860 | if (rule_attrs[GTP5G_FLOW_DESCRIPTION_SRC_MASK]) 861 | rule->smask.s_addr = nla_get_be32(rule_attrs[GTP5G_FLOW_DESCRIPTION_SRC_MASK]); 862 | else 863 | rule->smask.s_addr = -1; 864 | 865 | if (rule_attrs[GTP5G_FLOW_DESCRIPTION_DEST_MASK]) 866 | rule->dmask.s_addr = nla_get_be32(rule_attrs[GTP5G_FLOW_DESCRIPTION_DEST_MASK]); 867 | else 868 | rule->dmask.s_addr = -1; 869 | 870 | if (rule_attrs[GTP5G_FLOW_DESCRIPTION_SRC_PORT]) { 871 | u32 *sport_encode = nla_data(rule_attrs[GTP5G_FLOW_DESCRIPTION_SRC_PORT]); 872 | rule->sport_num = nla_len(rule_attrs[GTP5G_FLOW_DESCRIPTION_SRC_PORT]) / sizeof(u32); 873 | if (rule->sport) 874 | kfree(rule->sport); 875 | rule->sport = kzalloc(rule->sport_num * sizeof(*rule->sport), GFP_ATOMIC); 876 | if (!rule->sport) { 877 | GTP5G_ERR(NULL, "Failed to allocate SDF's Rule Source Port\n"); 878 | return -ENOMEM; 879 | } 880 | 881 | for (i = 0; i < rule->sport_num; i++) { 882 | if ((sport_encode[i] & 0xFFFF) <= (sport_encode[i] >> 16)) { 883 | rule->sport[i].start = (sport_encode[i] & 0xFFFF); 884 | rule->sport[i].end = (sport_encode[i] >> 16); 885 | } else { 886 | rule->sport[i].start = (sport_encode[i] >> 16); 887 | rule->sport[i].end = (sport_encode[i] & 0xFFFF); 888 | } 889 | } 890 | } 891 | 892 | if (rule_attrs[GTP5G_FLOW_DESCRIPTION_DEST_PORT]) { 893 | u32 *dport_encode = nla_data(rule_attrs[GTP5G_FLOW_DESCRIPTION_DEST_PORT]); 894 | rule->dport_num = nla_len(rule_attrs[GTP5G_FLOW_DESCRIPTION_DEST_PORT]) / sizeof(u32); 895 | 896 | if (rule->dport) 897 | kfree(rule->dport); 898 | 899 | rule->dport = kzalloc(rule->dport_num * sizeof(*rule->dport), GFP_ATOMIC); 900 | if (!rule->dport) { 901 | GTP5G_ERR(NULL, "Failed to allocate SDF's Rule Destination Port\n"); 902 | return -ENOMEM; 903 | } 904 | 905 | for (i = 0; i < rule->dport_num; i++) { 906 | if ((dport_encode[i] & 0xFFFF) <= (dport_encode[i] >> 16)) { 907 | rule->dport[i].start = (dport_encode[i] & 0xFFFF); 908 | rule->dport[i].end = (dport_encode[i] >> 16); 909 | } else { 910 | rule->dport[i].start = (dport_encode[i] >> 16); 911 | rule->dport[i].end = (dport_encode[i] & 0xFFFF); 912 | } 913 | } 914 | } 915 | } 916 | 917 | if (sdf_attrs[GTP5G_SDF_FILTER_TOS_TRAFFIC_CLASS]) { 918 | if (!sdf->tos_traffic_class) { 919 | sdf->tos_traffic_class = kzalloc(sizeof(*sdf->tos_traffic_class), GFP_ATOMIC); 920 | if (!sdf->tos_traffic_class) { 921 | GTP5G_ERR(NULL, "Failed to allocate SDF's TOS Traffic class\n"); 922 | return -ENOMEM; 923 | } 924 | } 925 | *sdf->tos_traffic_class = nla_get_u16(sdf_attrs[GTP5G_SDF_FILTER_TOS_TRAFFIC_CLASS]); 926 | } 927 | 928 | if (sdf_attrs[GTP5G_SDF_FILTER_SECURITY_PARAMETER_INDEX]) { 929 | if (!sdf->security_param_idx) { 930 | sdf->security_param_idx = kzalloc(sizeof(*sdf->security_param_idx), GFP_ATOMIC); 931 | if (!sdf->security_param_idx) { 932 | GTP5G_ERR(NULL, "Failed to allocate SDF's Security Param Index\n"); 933 | return -ENOMEM; 934 | } 935 | } 936 | *sdf->security_param_idx = nla_get_u32(sdf_attrs[GTP5G_SDF_FILTER_SECURITY_PARAMETER_INDEX]); 937 | } 938 | 939 | if (sdf_attrs[GTP5G_SDF_FILTER_FLOW_LABEL]) { 940 | if (!sdf->flow_label) { 941 | sdf->flow_label = kzalloc(sizeof(*sdf->flow_label), GFP_ATOMIC); 942 | if (!sdf->flow_label) { 943 | GTP5G_ERR(NULL, "Failed to allocate SDF's Flow label\n"); 944 | return -ENOMEM; 945 | } 946 | } 947 | *sdf->flow_label = nla_get_u32(sdf_attrs[GTP5G_SDF_FILTER_FLOW_LABEL]); 948 | } 949 | 950 | if (sdf_attrs[GTP5G_SDF_FILTER_SDF_FILTER_ID]) { 951 | if (!sdf->bi_id) { 952 | sdf->bi_id = kzalloc(sizeof(*sdf->bi_id), GFP_ATOMIC); 953 | if (!sdf->bi_id) { 954 | GTP5G_ERR(NULL, "Failed to allocate SDF's Filter id\n"); 955 | return -ENOMEM; 956 | } 957 | } 958 | *sdf->bi_id = nla_get_u32(sdf_attrs[GTP5G_SDF_FILTER_SDF_FILTER_ID]); 959 | } 960 | } 961 | } 962 | 963 | if (!hlist_unhashed(&pdr->hlist_i_teid)) 964 | hlist_del_rcu(&pdr->hlist_i_teid); 965 | 966 | if (!hlist_unhashed(&pdr->hlist_addr)) 967 | hlist_del_rcu(&pdr->hlist_addr); 968 | 969 | // Update hlist table 970 | if ((pdi = pdr->pdi)) { 971 | if ((f_teid = pdi->f_teid)) { 972 | last_ppdr = NULL; 973 | head = >p->i_teid_hash[u32_hashfn(f_teid->teid) % gtp->hash_size]; 974 | hlist_for_each_entry_rcu(ppdr, head, hlist_i_teid) { 975 | if (pdr->precedence > ppdr->precedence) 976 | last_ppdr = ppdr; 977 | else 978 | break; 979 | } 980 | 981 | if (!last_ppdr) 982 | hlist_add_head_rcu(&pdr->hlist_i_teid, head); 983 | else 984 | hlist_add_behind_rcu(&pdr->hlist_i_teid, &last_ppdr->hlist_i_teid); 985 | } else if (pdi->ue_addr_ipv4) { 986 | last_ppdr = NULL; 987 | head = >p->addr_hash[u32_hashfn(pdi->ue_addr_ipv4->s_addr) % gtp->hash_size]; 988 | hlist_for_each_entry_rcu(ppdr, head, hlist_addr) { 989 | if (pdr->precedence > ppdr->precedence) 990 | last_ppdr = ppdr; 991 | else 992 | break; 993 | } 994 | 995 | if (!last_ppdr) 996 | hlist_add_head_rcu(&pdr->hlist_addr, head); 997 | else 998 | hlist_add_behind_rcu(&pdr->hlist_addr, &last_ppdr->hlist_addr); 999 | } 1000 | } 1001 | 1002 | return 0; 1003 | } 1004 | 1005 | static int gtp5g_dev_init(struct net_device *dev) 1006 | { 1007 | struct gtp5g_dev *gtp = netdev_priv(dev); 1008 | 1009 | gtp->dev = dev; 1010 | 1011 | dev->tstats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats); 1012 | if (!dev->tstats) { 1013 | GTP5G_ERR(dev, "Failled to allocate stats\n"); 1014 | return -ENOMEM; 1015 | } 1016 | 1017 | return 0; 1018 | } 1019 | 1020 | static void __gtp5g_encap_destroy(struct sock *sk) 1021 | { 1022 | struct gtp5g_dev *gtp; 1023 | 1024 | lock_sock(sk); 1025 | gtp = sk->sk_user_data; 1026 | if (gtp) { 1027 | gtp->sk1u = NULL; 1028 | udp_sk(sk)->encap_type = 0; 1029 | rcu_assign_sk_user_data(sk, NULL); 1030 | sock_put(sk); 1031 | } 1032 | release_sock(sk); 1033 | } 1034 | 1035 | static void gtp5g_encap_disable_sock(struct sock *sk) 1036 | { 1037 | if (!sk) 1038 | return; 1039 | 1040 | __gtp5g_encap_destroy(sk); 1041 | } 1042 | 1043 | static void gtp5g_encap_disable(struct gtp5g_dev *gtp) 1044 | { 1045 | gtp5g_encap_disable_sock(gtp->sk1u); 1046 | } 1047 | 1048 | static void gtp5g_dev_uninit(struct net_device *dev) 1049 | { 1050 | struct gtp5g_dev *gtp = netdev_priv(dev); 1051 | 1052 | gtp5g_encap_disable(gtp); 1053 | free_percpu(dev->tstats); 1054 | } 1055 | 1056 | static void gtp5g_push_header(struct sk_buff *skb, struct gtp5g_pktinfo *pktinfo) 1057 | { 1058 | int payload_len = skb->len; 1059 | struct gtpv1_hdr *gtp1; 1060 | gtpv1_hdr_opt_t *gtp1opt; 1061 | ext_pdu_sess_ctr_t *dl_pdu_sess; 1062 | int ext_flag = 0; 1063 | 1064 | GTP5G_TRC(NULL, "SKBLen(%u) GTP-U V1(%zu) Opt(%zu) DL_PDU(%zu)\n", 1065 | payload_len, sizeof(*gtp1), sizeof(*gtp1opt), sizeof(*dl_pdu_sess)); 1066 | 1067 | pktinfo->gtph_port = pktinfo->hdr_creation->port; 1068 | 1069 | /* Suppport for extension header, sequence number and N-PDU. 1070 | * Update the length field if any of them is available. 1071 | */ 1072 | if (pktinfo->qer) { 1073 | ext_flag = 1; 1074 | 1075 | /* Push PDU Session container information */ 1076 | dl_pdu_sess = skb_push(skb, sizeof(*dl_pdu_sess)); 1077 | /* Multiple of 4 (TODO include PPI) */ 1078 | dl_pdu_sess->length = 1; 1079 | dl_pdu_sess->pdu_sess_ctr.type_spare = 0; /* For DL */ 1080 | dl_pdu_sess->pdu_sess_ctr.u.dl.ppp_rqi_qfi = pktinfo->qer->qfi; 1081 | //TODO: PPI 1082 | dl_pdu_sess->next_ehdr_type = 0; /* No more extension Header */ 1083 | 1084 | /* Push optional header information */ 1085 | gtp1opt = skb_push(skb, sizeof(*gtp1opt)); 1086 | gtp1opt->seq_number = 0; 1087 | gtp1opt->NPDU = 0; 1088 | gtp1opt->next_ehdr_type = 0x85; /* PDU Session Container */ 1089 | // Increment the GTP-U payload length by size of optional headers length 1090 | payload_len += (sizeof(*gtp1opt) + sizeof(*dl_pdu_sess)); 1091 | } 1092 | 1093 | /* Bits 8 7 6 5 4 3 2 1 1094 | * +--+--+--+--+--+--+--+--+ 1095 | * |version |PT| 0| E| S|PN| 1096 | * +--+--+--+--+--+--+--+--+ 1097 | * 0 0 1 1 0 0 0 0 1098 | */ 1099 | gtp1 = skb_push(skb, sizeof(*gtp1)); 1100 | gtp1->flags = 0x30; /* v1, GTP-non-prime. */ 1101 | if (ext_flag) 1102 | gtp1->flags |= GTPV1_HDR_FLG_EXTHDR; /* v1, Extension header enabled */ 1103 | gtp1->type = GTP_TPDU; 1104 | gtp1->tid = pktinfo->hdr_creation->teid; 1105 | gtp1->length = htons(payload_len); /* Excluded the header length of gtpv1 */ 1106 | 1107 | GTP5G_TRC(NULL, "QER Found GTP-U Flg(%u) GTPU-L(%u) SkbLen(%u)\n", 1108 | gtp1->flags, ntohs(gtp1->length), skb->len); 1109 | } 1110 | 1111 | static inline void gtp5g_set_pktinfo_ipv4(struct gtp5g_pktinfo *pktinfo, 1112 | struct sock *sk, struct iphdr *iph, 1113 | struct outer_header_creation *hdr_creation, 1114 | struct gtp5g_qer *qer, 1115 | struct rtable *rt, 1116 | struct flowi4 *fl4, 1117 | struct net_device *dev) 1118 | { 1119 | pktinfo->sk = sk; 1120 | pktinfo->iph = iph; 1121 | pktinfo->hdr_creation = hdr_creation; 1122 | pktinfo->qer = qer; 1123 | pktinfo->rt = rt; 1124 | pktinfo->fl4 = *fl4; 1125 | pktinfo->dev = dev; 1126 | } 1127 | 1128 | static struct rtable *ip4_find_route(struct sk_buff *skb, struct iphdr *iph, 1129 | struct sock *sk, struct net_device *gtp_dev, 1130 | __be32 saddr, __be32 daddr, struct flowi4 *fl4) 1131 | { 1132 | struct rtable *rt; 1133 | __be16 df; 1134 | int mtu; 1135 | 1136 | memset(fl4, 0, sizeof(*fl4)); 1137 | fl4->flowi4_oif = sk->sk_bound_dev_if; 1138 | fl4->daddr = daddr; 1139 | fl4->saddr = (saddr ? saddr : inet_sk(sk)->inet_saddr); 1140 | fl4->flowi4_tos = RT_CONN_FLAGS(sk); 1141 | fl4->flowi4_proto = sk->sk_protocol; 1142 | 1143 | rt = ip_route_output_key(dev_net(gtp_dev), fl4); 1144 | if (IS_ERR(rt)) { 1145 | GTP5G_ERR(gtp_dev, "no route to %pI4\n", &iph->daddr); 1146 | gtp_dev->stats.tx_carrier_errors++; 1147 | goto err; 1148 | } 1149 | 1150 | if (rt->dst.dev == gtp_dev) { 1151 | GTP5G_ERR(gtp_dev, "circular route to %pI4\n", &iph->daddr); 1152 | gtp_dev->stats.collisions++; 1153 | goto err_rt; 1154 | } 1155 | 1156 | skb_dst_drop(skb); 1157 | 1158 | /* This is similar to tnl_update_pmtu(). */ 1159 | df = iph->frag_off; 1160 | if (df) { 1161 | mtu = dst_mtu(&rt->dst) - gtp_dev->hard_header_len - 1162 | sizeof(struct iphdr) - sizeof(struct udphdr); 1163 | // GTPv1 1164 | mtu -= sizeof(struct gtpv1_hdr); 1165 | } 1166 | else { 1167 | mtu = dst_mtu(&rt->dst); 1168 | } 1169 | 1170 | #if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 4, 0) 1171 | rt->dst.ops->update_pmtu(&rt->dst, NULL, skb, mtu, false); 1172 | #else 1173 | rt->dst.ops->update_pmtu(&rt->dst, NULL, skb, mtu); 1174 | #endif 1175 | if (!skb_is_gso(skb) && (iph->frag_off & htons(IP_DF)) && 1176 | mtu < ntohs(iph->tot_len)) { 1177 | GTP5G_ERR(gtp_dev, "packet too big, fragmentation needed\n"); 1178 | memset(IPCB(skb), 0, sizeof(*IPCB(skb))); 1179 | icmp_send(skb, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED, 1180 | htonl(mtu)); 1181 | goto err_rt; 1182 | } 1183 | 1184 | return rt; 1185 | err_rt: 1186 | ip_rt_put(rt); 1187 | err: 1188 | return ERR_PTR(-ENOENT); 1189 | } 1190 | 1191 | static struct rtable *ip4_find_route_simple(struct sk_buff *skb, 1192 | struct sock *sk, struct net_device *gtp_dev, 1193 | __be32 saddr, __be32 daddr, struct flowi4 *fl4) 1194 | { 1195 | struct rtable *rt; 1196 | 1197 | memset(fl4, 0, sizeof(*fl4)); 1198 | fl4->flowi4_oif = sk->sk_bound_dev_if; 1199 | fl4->daddr = daddr; 1200 | fl4->saddr = (saddr ? saddr : inet_sk(sk)->inet_saddr); 1201 | fl4->flowi4_tos = RT_CONN_FLAGS(sk); 1202 | fl4->flowi4_proto = sk->sk_protocol; 1203 | 1204 | rt = ip_route_output_key(dev_net(gtp_dev), fl4); 1205 | if (IS_ERR(rt)) { 1206 | GTP5G_ERR(gtp_dev, "no route from %#x to %#x\n", saddr, daddr); 1207 | gtp_dev->stats.tx_carrier_errors++; 1208 | goto err; 1209 | } 1210 | 1211 | if (rt->dst.dev == gtp_dev) { 1212 | GTP5G_ERR(gtp_dev, "Packet colllisions from %#x to %#x\n", 1213 | saddr, daddr); 1214 | gtp_dev->stats.collisions++; 1215 | goto err_rt; 1216 | } 1217 | 1218 | skb_dst_drop(skb); 1219 | 1220 | return rt; 1221 | 1222 | err_rt: 1223 | ip_rt_put(rt); 1224 | err: 1225 | return ERR_PTR(-ENOENT); 1226 | } 1227 | 1228 | static int ip_xmit(struct sk_buff *skb, struct sock *sk, struct net_device *gtp_dev) 1229 | { 1230 | struct iphdr *iph = ip_hdr(skb); 1231 | struct flowi4 fl4; 1232 | struct rtable *rt; 1233 | 1234 | rt = ip4_find_route(skb, iph, sk, gtp_dev, 0, iph->daddr, &fl4); 1235 | if (IS_ERR(rt)) { 1236 | GTP5G_ERR(gtp_dev, "Failed to find route\n"); 1237 | return -EBADMSG; 1238 | } 1239 | 1240 | skb_dst_set(skb, &rt->dst); 1241 | 1242 | if (ip_local_out(dev_net(gtp_dev), sk, skb) < 0) { 1243 | GTP5G_ERR(gtp_dev, "Failed to send skb to ip layer\n"); 1244 | return -1; 1245 | } 1246 | return 0; 1247 | } 1248 | 1249 | static int gtp5g_drop_skb_ipv4(struct sk_buff *skb, struct net_device *dev) 1250 | { 1251 | dev->stats.tx_dropped++; 1252 | dev_kfree_skb(skb); 1253 | return FAR_ACTION_DROP; 1254 | } 1255 | 1256 | static void gtp5g_fwd_emark_skb_ipv4(struct sk_buff *skb, 1257 | struct net_device *dev, 1258 | struct gtp5g_emark_pktinfo *epkt_info) { 1259 | struct rtable *rt; 1260 | struct flowi4 fl4; 1261 | struct gtpv1_hdr *gtp1; 1262 | 1263 | /* Reset all headers */ 1264 | skb_reset_transport_header(skb); 1265 | skb_reset_network_header(skb); 1266 | skb_reset_mac_header(skb); 1267 | 1268 | /* Fill GTP-U Header */ 1269 | gtp1 = skb_push(skb, sizeof(*gtp1)); 1270 | gtp1->flags = 0x30; /* v1, GTP-non-prime. */ 1271 | gtp1->type = GTP_EMARK; 1272 | gtp1->tid = epkt_info->teid; 1273 | 1274 | rt = ip4_find_route_simple(skb, epkt_info->sk, dev, 1275 | epkt_info->role_addr /* Src Addr */ , 1276 | epkt_info->peer_addr /* Dst Addr*/, 1277 | &fl4); 1278 | if (IS_ERR(rt)) { 1279 | GTP5G_ERR(dev, "Failed to send GTP-U end-marker due to routing\n"); 1280 | return; 1281 | } 1282 | 1283 | udp_tunnel_xmit_skb(rt, 1284 | epkt_info->sk, 1285 | skb, 1286 | fl4.saddr, 1287 | fl4.daddr, 1288 | 0, 1289 | ip4_dst_hoplimit(&rt->dst), 1290 | 0, 1291 | epkt_info->gtph_port, 1292 | epkt_info->gtph_port, 1293 | true, 1294 | true); 1295 | } 1296 | 1297 | static int gtp5g_fwd_skb_ipv4(struct sk_buff *skb, 1298 | struct net_device *dev, 1299 | struct gtp5g_pktinfo *pktinfo, 1300 | struct gtp5g_pdr *pdr) 1301 | { 1302 | struct rtable *rt; 1303 | struct flowi4 fl4; 1304 | struct iphdr *iph = ip_hdr(skb); 1305 | struct outer_header_creation *hdr_creation; 1306 | 1307 | if (!(pdr->far && pdr->far->fwd_param && pdr->far->fwd_param->hdr_creation)) { 1308 | GTP5G_ERR(dev, "Unknown RAN address\n"); 1309 | dev->stats.tx_carrier_errors++; 1310 | goto err; 1311 | } 1312 | 1313 | hdr_creation = pdr->far->fwd_param->hdr_creation; 1314 | rt = ip4_find_route(skb, iph, pdr->sk, dev, 1315 | pdr->role_addr_ipv4.s_addr, 1316 | hdr_creation->peer_addr_ipv4.s_addr, 1317 | &fl4); 1318 | if (IS_ERR(rt)) 1319 | goto err; 1320 | 1321 | if (!pdr->qer) { 1322 | gtp5g_set_pktinfo_ipv4(pktinfo, pdr->sk, iph, hdr_creation, NULL, rt, &fl4, dev); 1323 | } else { 1324 | gtp5g_set_pktinfo_ipv4(pktinfo, pdr->sk, iph, hdr_creation, pdr->qer, rt, &fl4, dev); 1325 | } 1326 | 1327 | gtp5g_push_header(skb, pktinfo); 1328 | 1329 | return FAR_ACTION_FORW; 1330 | 1331 | err: 1332 | return -EBADMSG; 1333 | } 1334 | 1335 | static int gtp5g_buf_skb_ipv4(struct sk_buff *skb, struct net_device *dev, 1336 | struct gtp5g_pdr *pdr) 1337 | { 1338 | // TODO: handle nonlinear part 1339 | if (unix_sock_send(pdr, skb->data, skb_headlen(skb)) < 0) 1340 | GTP5G_ERR(dev, "Failed to send skb to unix domain socket PDR(%u)", pdr->id); 1341 | 1342 | dev_kfree_skb(skb); 1343 | return FAR_ACTION_BUFF; 1344 | } 1345 | 1346 | static int gtp5g_handle_skb_ipv4(struct sk_buff *skb, struct net_device *dev, 1347 | struct gtp5g_pktinfo *pktinfo) 1348 | { 1349 | struct gtp5g_dev *gtp = netdev_priv(dev); 1350 | struct gtp5g_pdr *pdr; 1351 | struct gtp5g_far *far; 1352 | //struct gtp5g_qer *qer; 1353 | struct iphdr *iph; 1354 | 1355 | /* Read the IP destination address and resolve the PDR. 1356 | * Prepend PDR header with TEI/TID from PDR. 1357 | */ 1358 | iph = ip_hdr(skb); 1359 | if (gtp->role == GTP5G_ROLE_UPF) 1360 | pdr = pdr_find_by_ipv4(gtp, skb, 0, iph->daddr); 1361 | else 1362 | pdr = pdr_find_by_ipv4(gtp, skb, 0, iph->saddr); 1363 | 1364 | if (!pdr) { 1365 | GTP5G_ERR(dev, "no PDR found for %pI4, skip\n", 1366 | &iph->daddr); 1367 | return -ENOENT; 1368 | } 1369 | //GTP5G_ERR(dev, "found PDR %p\n", pdr); 1370 | 1371 | /* TODO: QoS rule have to apply before apply FAR 1372 | * */ 1373 | //qer = pdr->qer; 1374 | //if (qer) { 1375 | // GTP5G_ERR(dev, "%s:%d QER Rule found, id(%#x) qfi(%#x) TODO\n", 1376 | // __func__, __LINE__, qer->id, qer->qfi); 1377 | //} 1378 | 1379 | far = pdr->far; 1380 | if (far) { 1381 | // One and only one of the DROP, FORW and BUFF flags shall be set to 1. 1382 | // The NOCP flag may only be set if the BUFF flag is set. 1383 | // The DUPL flag may be set with any of the DROP, FORW, BUFF and NOCP flags. 1384 | switch (far->action & FAR_ACTION_MASK) { 1385 | case FAR_ACTION_DROP: 1386 | return gtp5g_drop_skb_ipv4(skb, dev); 1387 | case FAR_ACTION_FORW: 1388 | return gtp5g_fwd_skb_ipv4(skb, dev, pktinfo, pdr); 1389 | case FAR_ACTION_BUFF: 1390 | return gtp5g_buf_skb_ipv4(skb, dev, pdr); 1391 | default: 1392 | GTP5G_ERR(dev, "Unspec apply action[%u] in FAR[%u] and related to PDR[%u]", 1393 | far->action, far->id, pdr->id); 1394 | } 1395 | } 1396 | 1397 | return -ENOENT; 1398 | } 1399 | 1400 | static void gtp5g_xmit_skb_ipv4(struct sk_buff *skb, struct gtp5g_pktinfo *pktinfo) 1401 | { 1402 | //GTP5G_ERR(pktinfo->dev, "gtp -> IP src: %pI4 dst: %pI4\n", 1403 | // &pktinfo->iph->saddr, &pktinfo->iph->daddr); 1404 | udp_tunnel_xmit_skb(pktinfo->rt, pktinfo->sk, skb, 1405 | pktinfo->fl4.saddr, pktinfo->fl4.daddr, 1406 | pktinfo->iph->tos, 1407 | ip4_dst_hoplimit(&pktinfo->rt->dst), 1408 | 0, 1409 | pktinfo->gtph_port, pktinfo->gtph_port, 1410 | true, true); 1411 | } 1412 | 1413 | /** 1414 | * Entry function for Downlink packets 1415 | * */ 1416 | static netdev_tx_t gtp5g_dev_xmit(struct sk_buff *skb, struct net_device *dev) 1417 | { 1418 | unsigned int proto = ntohs(skb->protocol); 1419 | struct gtp5g_pktinfo pktinfo; 1420 | int ret; 1421 | 1422 | /* Ensure there is sufficient headroom. */ 1423 | if (skb_cow_head(skb, dev->needed_headroom)) 1424 | goto tx_err; 1425 | 1426 | skb_reset_inner_headers(skb); 1427 | 1428 | /* PDR lookups in gtp5g_build_skb_*() need rcu read-side lock. */ 1429 | rcu_read_lock(); 1430 | switch (proto) { 1431 | case ETH_P_IP: 1432 | //printk_ratelimited("%s:%d Dowlink Packet received, skb.len(%u)\n", 1433 | // __func__, __LINE__, skb->len); 1434 | ret = gtp5g_handle_skb_ipv4(skb, dev, &pktinfo); 1435 | //printk_ratelimited("%s:%d Dowlink Packet processed ret(%u) skb.len(%u)\n", 1436 | // __func__, __LINE__, ret, skb->len); 1437 | break; 1438 | default: 1439 | ret = -EOPNOTSUPP; 1440 | } 1441 | rcu_read_unlock(); 1442 | 1443 | if (ret < 0) 1444 | goto tx_err; 1445 | 1446 | if (ret == FAR_ACTION_FORW) 1447 | gtp5g_xmit_skb_ipv4(skb, &pktinfo); 1448 | 1449 | return NETDEV_TX_OK; 1450 | 1451 | tx_err: 1452 | dev->stats.tx_errors++; 1453 | dev_kfree_skb(skb); 1454 | return NETDEV_TX_OK; 1455 | } 1456 | 1457 | static const struct net_device_ops gtp5g_netdev_ops = { 1458 | .ndo_init = gtp5g_dev_init, 1459 | .ndo_uninit = gtp5g_dev_uninit, 1460 | .ndo_start_xmit = gtp5g_dev_xmit, 1461 | .ndo_get_stats64 = ip_tunnel_get_stats64, 1462 | }; 1463 | 1464 | static void pdr_context_free(struct rcu_head *head) 1465 | { 1466 | struct gtp5g_pdr *pdr = container_of(head, struct gtp5g_pdr, rcu_head); 1467 | struct gtp5g_pdi *pdi; 1468 | struct sdf_filter *sdf; 1469 | 1470 | if (!pdr) 1471 | return; 1472 | 1473 | sock_put(pdr->sk); 1474 | 1475 | if (pdr->outer_header_removal) kfree(pdr->outer_header_removal); 1476 | 1477 | pdi = pdr->pdi; 1478 | if (pdi) { 1479 | if (pdi->ue_addr_ipv4) 1480 | kfree(pdi->ue_addr_ipv4); 1481 | if (pdi->f_teid) 1482 | kfree(pdi->f_teid); 1483 | if (pdr->pdi) 1484 | kfree(pdr->pdi); 1485 | if (pdr->far_id) 1486 | kfree(pdr->far_id); 1487 | if (pdr->qer_id) 1488 | kfree(pdr->qer_id); 1489 | 1490 | sdf = pdi->sdf; 1491 | if (sdf) { 1492 | if (sdf->rule) { 1493 | if (sdf->rule->sport) 1494 | kfree(sdf->rule->sport); 1495 | if (sdf->rule->dport) 1496 | kfree(sdf->rule->dport); 1497 | if (sdf->rule) 1498 | kfree(sdf->rule); 1499 | } 1500 | if (sdf->tos_traffic_class) 1501 | kfree(sdf->tos_traffic_class); 1502 | if (sdf->security_param_idx) 1503 | kfree(sdf->security_param_idx); 1504 | if (sdf->flow_label) 1505 | kfree(sdf->flow_label); 1506 | if (sdf->bi_id) 1507 | kfree(sdf->bi_id); 1508 | } 1509 | } 1510 | 1511 | unix_sock_client_delete(pdr); 1512 | kfree(pdr); 1513 | } 1514 | 1515 | static void pdr_context_delete(struct gtp5g_pdr *pdr) 1516 | { 1517 | if (!pdr) 1518 | return; 1519 | 1520 | if (!hlist_unhashed(&pdr->hlist_id)) 1521 | hlist_del_rcu(&pdr->hlist_id); 1522 | 1523 | if (!hlist_unhashed(&pdr->hlist_i_teid)) 1524 | hlist_del_rcu(&pdr->hlist_i_teid); 1525 | 1526 | if (!hlist_unhashed(&pdr->hlist_addr)) 1527 | hlist_del_rcu(&pdr->hlist_addr); 1528 | 1529 | if (!hlist_unhashed(&pdr->hlist_related_far)) 1530 | hlist_del_rcu(&pdr->hlist_related_far); 1531 | 1532 | if (!hlist_unhashed(&pdr->hlist_related_qer)) 1533 | hlist_del_rcu(&pdr->hlist_related_qer); 1534 | 1535 | call_rcu(&pdr->rcu_head, pdr_context_free); 1536 | } 1537 | 1538 | static void far_context_free(struct rcu_head *head) 1539 | { 1540 | struct gtp5g_far *far = container_of(head, struct gtp5g_far, rcu_head); 1541 | struct forwarding_parameter *fwd_param = far->fwd_param; 1542 | 1543 | if (!far) 1544 | return; 1545 | 1546 | if (fwd_param) { 1547 | if (fwd_param->hdr_creation) 1548 | kfree(fwd_param->hdr_creation); 1549 | if (fwd_param->fwd_policy) 1550 | kfree(fwd_param->fwd_policy); 1551 | } 1552 | 1553 | if (fwd_param) 1554 | kfree(fwd_param); 1555 | 1556 | kfree(far); 1557 | } 1558 | 1559 | static void far_context_delete(struct gtp5g_far *far) 1560 | { 1561 | struct gtp5g_dev *gtp = netdev_priv(far->dev); 1562 | struct hlist_head *head; 1563 | struct gtp5g_pdr *pdr; 1564 | 1565 | if (!far) 1566 | return; 1567 | 1568 | if (!hlist_unhashed(&far->hlist_id)) 1569 | hlist_del_rcu(&far->hlist_id); 1570 | 1571 | head = >p->related_far_hash[u32_hashfn(far->id) % gtp->hash_size]; 1572 | hlist_for_each_entry_rcu(pdr, head, hlist_related_far) { 1573 | if (*pdr->far_id == far->id) { 1574 | pdr->far = NULL; 1575 | unix_sock_client_delete(pdr); 1576 | } 1577 | } 1578 | 1579 | call_rcu(&far->rcu_head, far_context_free); 1580 | } 1581 | 1582 | static int gtp5g_hashtable_new(struct gtp5g_dev *gtp, int hsize) 1583 | { 1584 | int i; 1585 | 1586 | gtp->addr_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1587 | GFP_KERNEL); 1588 | if (gtp->addr_hash == NULL) 1589 | return -ENOMEM; 1590 | 1591 | gtp->i_teid_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1592 | GFP_KERNEL); 1593 | if (gtp->i_teid_hash == NULL) 1594 | goto err1; 1595 | 1596 | gtp->pdr_id_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1597 | GFP_KERNEL); 1598 | if (gtp->pdr_id_hash == NULL) 1599 | goto err2; 1600 | 1601 | gtp->far_id_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1602 | GFP_KERNEL); 1603 | if (gtp->far_id_hash == NULL) 1604 | goto err3; 1605 | 1606 | gtp->qer_id_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1607 | GFP_KERNEL); 1608 | if (gtp->qer_id_hash == NULL) 1609 | goto err4; 1610 | 1611 | gtp->related_far_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1612 | GFP_KERNEL); 1613 | if (gtp->related_far_hash == NULL) 1614 | goto err5; 1615 | 1616 | gtp->related_qer_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1617 | GFP_KERNEL); 1618 | if (gtp->related_qer_hash == NULL) 1619 | goto err6; 1620 | 1621 | 1622 | gtp->hash_size = hsize; 1623 | 1624 | for (i = 0; i < hsize; i++) { 1625 | INIT_HLIST_HEAD(>p->addr_hash[i]); 1626 | INIT_HLIST_HEAD(>p->i_teid_hash[i]); 1627 | INIT_HLIST_HEAD(>p->pdr_id_hash[i]); 1628 | INIT_HLIST_HEAD(>p->far_id_hash[i]); 1629 | INIT_HLIST_HEAD(>p->qer_id_hash[i]); 1630 | INIT_HLIST_HEAD(>p->related_far_hash[i]); 1631 | INIT_HLIST_HEAD(>p->related_qer_hash[i]); 1632 | } 1633 | 1634 | return 0; 1635 | 1636 | err6: 1637 | kfree(gtp->related_far_hash); 1638 | err5: 1639 | kfree(gtp->qer_id_hash); 1640 | err4: 1641 | kfree(gtp->far_id_hash); 1642 | err3: 1643 | kfree(gtp->pdr_id_hash); 1644 | err2: 1645 | kfree(gtp->i_teid_hash); 1646 | err1: 1647 | kfree(gtp->addr_hash); 1648 | return -ENOMEM; 1649 | } 1650 | 1651 | static void gtp5g_hashtable_free(struct gtp5g_dev *gtp) 1652 | { 1653 | struct gtp5g_pdr *pdr; 1654 | struct gtp5g_far *far; 1655 | struct gtp5g_qer *qer; 1656 | int i; 1657 | 1658 | for (i = 0; i < gtp->hash_size; i++) { 1659 | hlist_for_each_entry_rcu(pdr, >p->pdr_id_hash[i], hlist_id) 1660 | pdr_context_delete(pdr); 1661 | hlist_for_each_entry_rcu(far, >p->far_id_hash[i], hlist_id) 1662 | far_context_delete(far); 1663 | hlist_for_each_entry_rcu(qer, >p->qer_id_hash[i], hlist_id) 1664 | qer_context_delete(qer); 1665 | } 1666 | 1667 | synchronize_rcu(); 1668 | kfree(gtp->addr_hash); 1669 | kfree(gtp->i_teid_hash); 1670 | kfree(gtp->pdr_id_hash); 1671 | kfree(gtp->far_id_hash); 1672 | kfree(gtp->qer_id_hash); 1673 | kfree(gtp->related_far_hash); 1674 | kfree(gtp->related_qer_hash); 1675 | } 1676 | 1677 | static void gtp5g_link_setup(struct net_device *dev) 1678 | { 1679 | dev->netdev_ops = >p5g_netdev_ops; 1680 | dev->needs_free_netdev = true; 1681 | 1682 | dev->hard_header_len = 0; 1683 | dev->addr_len = 0; 1684 | dev->mtu = ETH_DATA_LEN - 1685 | (sizeof(struct iphdr) + 1686 | sizeof(struct udphdr) + 1687 | sizeof(struct gtpv1_hdr)); 1688 | 1689 | /* Zero header length. */ 1690 | dev->type = ARPHRD_NONE; 1691 | dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST; 1692 | 1693 | dev->priv_flags |= IFF_NO_QUEUE; 1694 | dev->features |= NETIF_F_LLTX; 1695 | netif_keep_dst(dev); 1696 | 1697 | /* TODO: Modify the headroom size based on 1698 | * what are the extension header going to supports. 1699 | * */ 1700 | dev->needed_headroom = LL_MAX_HEADER + 1701 | sizeof(struct iphdr) + 1702 | sizeof(struct udphdr) + 1703 | sizeof(struct gtpv1_hdr) + 1704 | sizeof(struct gtp1_hdr_opt) + 1705 | sizeof(struct gtp1_hdr_ext_pdu_sess_ctr); 1706 | } 1707 | 1708 | static int gtp5g_validate(struct nlattr *tb[], struct nlattr *data[], 1709 | struct netlink_ext_ack *extack) 1710 | { 1711 | if (!data) 1712 | return -EINVAL; 1713 | 1714 | return 0; 1715 | } 1716 | 1717 | static struct gtp5g_dev *gtp5g_find_dev(struct net *src_net, struct nlattr *nla[]) 1718 | { 1719 | struct gtp5g_dev *gtp = NULL; 1720 | struct net_device *dev; 1721 | struct net *net; 1722 | 1723 | /* Examine the link attributes and figure out which network namespace 1724 | * we are talking about. 1725 | */ 1726 | if (nla[GTP5G_NET_NS_FD]) 1727 | net = get_net_ns_by_fd(nla_get_u32(nla[GTP5G_NET_NS_FD])); 1728 | else 1729 | net = get_net(src_net); 1730 | 1731 | if (IS_ERR(net)) 1732 | return NULL; 1733 | 1734 | /* Check if there's an existing gtp5g device to configure */ 1735 | dev = dev_get_by_index_rcu(net, nla_get_u32(nla[GTP5G_LINK])); 1736 | if (dev && dev->netdev_ops == >p5g_netdev_ops) 1737 | gtp = netdev_priv(dev); 1738 | 1739 | put_net(net); 1740 | 1741 | return gtp; 1742 | } 1743 | 1744 | static struct gtp5g_pdr *gtp5g_find_pdr_by_link(struct net *net, struct nlattr *nla[]) 1745 | { 1746 | struct gtp5g_dev *gtp; 1747 | 1748 | gtp = gtp5g_find_dev(net, nla); 1749 | if (!gtp) { 1750 | GTP5G_ERR(NULL, "Failed to find gtp device\n"); 1751 | return ERR_PTR(-ENODEV); 1752 | } 1753 | 1754 | if (nla[GTP5G_PDR_ID]) { 1755 | u16 id = nla_get_u16(nla[GTP5G_PDR_ID]); 1756 | return pdr_find_by_id(gtp, id); 1757 | } 1758 | 1759 | return ERR_PTR(-EINVAL); 1760 | } 1761 | 1762 | static struct gtp5g_pdr *gtp5g_find_pdr(struct net *net, struct nlattr *nla[]) 1763 | { 1764 | struct gtp5g_pdr *pdr; 1765 | 1766 | if (nla[GTP5G_LINK]) 1767 | pdr = gtp5g_find_pdr_by_link(net, nla); 1768 | else 1769 | pdr = ERR_PTR(-EINVAL); 1770 | 1771 | if (!pdr) 1772 | pdr = ERR_PTR(-ENOENT); 1773 | 1774 | return pdr; 1775 | } 1776 | 1777 | static struct gtp5g_far *gtp5g_find_far_by_link(struct net *net, struct nlattr *nla[]) 1778 | { 1779 | struct gtp5g_dev *gtp; 1780 | 1781 | gtp = gtp5g_find_dev(net, nla); 1782 | if (!gtp) { 1783 | GTP5G_ERR(NULL, "Failed to find gtp device\n"); 1784 | return ERR_PTR(-ENODEV); 1785 | } 1786 | 1787 | if (nla[GTP5G_FAR_ID]) { 1788 | u32 id = nla_get_u32(nla[GTP5G_FAR_ID]); 1789 | return far_find_by_id(gtp, id); 1790 | } 1791 | 1792 | return ERR_PTR(-EINVAL); 1793 | } 1794 | 1795 | static struct gtp5g_far *gtp5g_find_far(struct net *net, struct nlattr *nla[]) 1796 | { 1797 | struct gtp5g_far *far = NULL; 1798 | 1799 | if (nla[GTP5G_LINK]) 1800 | far = gtp5g_find_far_by_link(net, nla); 1801 | else 1802 | far = ERR_PTR(-EINVAL); 1803 | 1804 | if (!far) 1805 | far = ERR_PTR(-ENOENT); 1806 | 1807 | return far; 1808 | } 1809 | 1810 | static void gtp5g_encap_destroy(struct sock *sk) 1811 | { 1812 | rtnl_lock(); 1813 | __gtp5g_encap_destroy(sk); 1814 | rtnl_unlock(); 1815 | } 1816 | 1817 | static struct gtp5g_pdr *pdr_find_by_gtp1u(struct gtp5g_dev *gtp, struct sk_buff *skb, 1818 | unsigned int hdrlen, u32 teid) 1819 | { 1820 | struct iphdr *iph; 1821 | __be32 *target_addr; 1822 | struct hlist_head *head; 1823 | struct gtp5g_pdr *pdr; 1824 | struct gtp5g_pdi *pdi; 1825 | 1826 | switch(ntohs(skb->protocol)) { 1827 | case ETH_P_IP: 1828 | break; 1829 | default: 1830 | return NULL; 1831 | } 1832 | 1833 | if (!pskb_may_pull(skb, hdrlen + sizeof(struct iphdr))) { 1834 | GTP5G_ERR(NULL, "Failed to pull skb\n"); 1835 | return NULL; 1836 | } 1837 | 1838 | iph = (struct iphdr *)(skb->data + hdrlen); 1839 | target_addr = (gtp->role == GTP5G_ROLE_UPF ? &iph->saddr : &iph->daddr); 1840 | 1841 | head = >p->i_teid_hash[u32_hashfn(teid) % gtp->hash_size]; 1842 | hlist_for_each_entry_rcu(pdr, head, hlist_i_teid) { 1843 | pdi = pdr->pdi; 1844 | 1845 | // GTP-U packet must check teid 1846 | if (!(pdi->f_teid && pdi->f_teid->teid == teid)) 1847 | continue; 1848 | 1849 | if (pdi->ue_addr_ipv4) 1850 | if (!(pdr->af == AF_INET && *target_addr == pdi->ue_addr_ipv4->s_addr)) 1851 | continue; 1852 | 1853 | if (pdi->sdf) 1854 | if (!sdf_filter_match(pdi->sdf, skb, hdrlen, GTP5G_SDF_FILTER_OUT)) 1855 | continue; 1856 | 1857 | return pdr; 1858 | } 1859 | 1860 | return NULL; 1861 | } 1862 | 1863 | static int gtp5g_drop_skb_encap(struct sk_buff *skb, struct net_device *dev) 1864 | { 1865 | dev->stats.tx_dropped++; 1866 | dev_kfree_skb(skb); 1867 | return 0; 1868 | } 1869 | 1870 | static int gtp5g_fwd_skb_encap(struct sk_buff *skb, struct net_device *dev, 1871 | unsigned int hdrlen, struct gtp5g_pdr *pdr) 1872 | { 1873 | struct gtp5g_far *far = pdr->far; 1874 | struct forwarding_parameter *fwd_param = far->fwd_param; 1875 | struct outer_header_creation *hdr_creation; 1876 | struct forwarding_policy *fwd_policy; 1877 | 1878 | struct gtpv1_hdr *gtp1; 1879 | struct iphdr *iph; 1880 | struct udphdr *uh; 1881 | 1882 | struct pcpu_sw_netstats *stats; 1883 | 1884 | if (fwd_param) { 1885 | if ((fwd_policy = fwd_param->fwd_policy)) 1886 | skb->mark = fwd_policy->mark; 1887 | 1888 | if ((hdr_creation = fwd_param->hdr_creation)) { 1889 | // Just modify the teid and packet dest ip 1890 | gtp1 = (struct gtpv1_hdr *)(skb->data + sizeof(struct udphdr)); 1891 | gtp1->tid = hdr_creation->teid; 1892 | 1893 | skb_push(skb, 20); // L3 Header Length 1894 | iph = ip_hdr(skb); 1895 | 1896 | if (!pdr->pdi->f_teid) { 1897 | pr_err("Unable to handle hdr removal + creation " 1898 | "due to pdr->pdi->f_teid not exist\n"); 1899 | return -1; 1900 | } 1901 | 1902 | iph->saddr = pdr->pdi->f_teid->gtpu_addr_ipv4.s_addr; 1903 | iph->daddr = hdr_creation->peer_addr_ipv4.s_addr; 1904 | iph->check = 0; 1905 | 1906 | uh = udp_hdr(skb); 1907 | uh->check = 0; 1908 | 1909 | if (ip_xmit(skb, pdr->sk, dev) < 0) { 1910 | pr_err("ip_xmit error\n"); 1911 | return -1; 1912 | } 1913 | 1914 | return 0; 1915 | } 1916 | } 1917 | 1918 | // Get rid of the GTP + UDP headers. 1919 | if (iptunnel_pull_header(skb, hdrlen, skb->protocol, 1920 | !net_eq(sock_net(pdr->sk), dev_net(dev)))) 1921 | return -1; 1922 | 1923 | /* Now that the UDP and the GTP header have been removed, set up the 1924 | * new network header. This is required by the upper layer to 1925 | * calculate the transport header. 1926 | */ 1927 | skb_reset_network_header(skb); 1928 | 1929 | skb->dev = dev; 1930 | 1931 | stats = this_cpu_ptr(skb->dev->tstats); 1932 | u64_stats_update_begin(&stats->syncp); 1933 | stats->rx_packets++; 1934 | stats->rx_bytes += skb->len; 1935 | u64_stats_update_end(&stats->syncp); 1936 | 1937 | netif_rx(skb); 1938 | 1939 | return 0; 1940 | } 1941 | 1942 | static int gtp5g_buf_skb_encap(struct sk_buff *skb, struct net_device *dev, struct gtp5g_pdr *pdr) 1943 | { 1944 | if (unix_sock_send(pdr, skb->data, skb_headlen(skb)) < 0) 1945 | GTP5G_ERR(dev, "Failed to send skb to unix domain socket PDR(%u)", pdr->id); 1946 | 1947 | dev_kfree_skb(skb); 1948 | return 0; 1949 | } 1950 | 1951 | static int gtp5g_rx(struct gtp5g_pdr *pdr, struct sk_buff *skb, 1952 | unsigned int hdrlen, unsigned int role) 1953 | { 1954 | int rt; 1955 | struct gtp5g_far *far = pdr->far; 1956 | //struct gtp5g_qer *qer = pdr->qer; 1957 | 1958 | if (!far) { 1959 | pr_err("There is no FAR related to PDR(%u)", pdr->id); 1960 | return -1; 1961 | } 1962 | 1963 | //TODO: QER 1964 | //if (qer) { 1965 | // printk_ratelimited("%s:%d QER Rule found, id(%#x) qfi(%#x)\n", __func__, __LINE__, 1966 | // qer->id, qer->qfi); 1967 | //} 1968 | 1969 | // TODO: not reading the value of outer_header_removal now, 1970 | // just check if it is assigned. 1971 | if (pdr->outer_header_removal) { 1972 | // One and only one of the DROP, FORW and BUFF flags shall be set to 1. 1973 | // The NOCP flag may only be set if the BUFF flag is set. 1974 | // The DUPL flag may be set with any of the DROP, FORW, BUFF and NOCP flags. 1975 | switch(far->action & FAR_ACTION_MASK) { 1976 | case FAR_ACTION_DROP: 1977 | rt = gtp5g_drop_skb_encap(skb, pdr->dev); 1978 | break; 1979 | case FAR_ACTION_FORW: 1980 | rt = gtp5g_fwd_skb_encap(skb, pdr->dev, hdrlen, pdr); 1981 | break; 1982 | case FAR_ACTION_BUFF: 1983 | rt = gtp5g_buf_skb_encap(skb, pdr->dev, pdr); 1984 | break; 1985 | default: 1986 | pr_err("Unspec apply action[%u] in FAR[%u] and related to PDR[%u]", 1987 | far->action, far->id, pdr->id); 1988 | rt = -1; 1989 | } 1990 | } else { 1991 | // TODO: this action is not supported 1992 | pr_err("Unsupported action: not removing outer hdr of a non-gtp packet " 1993 | "(which routed to the gtp interface and matches a PDR)"); 1994 | return -1; 1995 | } 1996 | 1997 | return rt; 1998 | } 1999 | 2000 | static int get_gtpu_header_len(struct gtpv1_hdr *gtpv1, u16 prefix_hdrlen) 2001 | { 2002 | u16 len = sizeof(*gtpv1); 2003 | 2004 | /** TS 29.281 Chapter 5.1 and Figure 5.1-1 2005 | * GTP-U header at least 8 byte 2006 | * 2007 | * This field shall be present if and only if any one or more of the S, PN and E flags are set. 2008 | * This field means seq number (2 Octect), N-PDU number (1 Octet) and Next ext hdr type (1 Octet). 2009 | * 2010 | * TODO: Validate the Reserved flag set or not, if it is set then protocol error 2011 | */ 2012 | if (gtpv1->flags & GTPV1_HDR_FLG_MASK) 2013 | len += 4; 2014 | else 2015 | return len; 2016 | 2017 | /** TS 29.281 Chapter 5.2 and Figure 5.2.1-1 2018 | * The length of the Extension header shall be defined in a variable length of 4 octets, 2019 | * i.e. m+1 = n*4 octets, where n is a positive integer. 2020 | */ 2021 | if (gtpv1->flags & GTPV1_HDR_FLG_EXTHDR) { 2022 | __u8 next_ehdr_type = 0; 2023 | gtpv1_hdr_opt_t *gtpv1_opt = (gtpv1_hdr_opt_t *) ((u8 *) gtpv1 + sizeof(*gtpv1)); 2024 | 2025 | next_ehdr_type = gtpv1_opt->next_ehdr_type; 2026 | while (next_ehdr_type) { 2027 | switch (next_ehdr_type) { 2028 | case GTPV1_NEXT_EXT_HDR_TYPE_85: 2029 | { 2030 | ext_pdu_sess_ctr_t *etype85 = (ext_pdu_sess_ctr_t *) ((u8 *) gtpv1_opt + sizeof(*gtpv1_opt)); 2031 | pdu_sess_ctr_t *pdu_sess_info = &etype85->pdu_sess_ctr; 2032 | 2033 | //printk_ratelimited("%s: GTPV1_NEXT_EXT_HDR_TYPE_85\n", __func__); 2034 | 2035 | if (pdu_sess_info->type_spare == PDU_SESSION_INFO_TYPE0) 2036 | return -1; 2037 | 2038 | //TODO: validate pdu_sess_ctr 2039 | 2040 | //Length should be multiple of 4 2041 | len += (etype85->length * 4); 2042 | next_ehdr_type = etype85->next_ehdr_type; 2043 | break; 2044 | } 2045 | 2046 | default: 2047 | /* Unknown/Unhandled Extension Header Type */ 2048 | printk_ratelimited("%s: Invalid header type(%#x)\n", __func__, next_ehdr_type); 2049 | return -1; 2050 | } 2051 | } 2052 | } 2053 | 2054 | return len; 2055 | } 2056 | 2057 | static int gtp1u_udp_encap_recv(struct gtp5g_dev *gtp, struct sk_buff *skb) 2058 | { 2059 | unsigned int hdrlen = sizeof(struct udphdr) + sizeof(struct gtpv1_hdr); 2060 | struct gtpv1_hdr *gtpv1; 2061 | struct gtp5g_pdr *pdr; 2062 | int gtpv1_hdr_len; 2063 | 2064 | if (!pskb_may_pull(skb, hdrlen)) 2065 | return -1; 2066 | 2067 | gtpv1 = (struct gtpv1_hdr *)(skb->data + sizeof(struct udphdr)); 2068 | if ((gtpv1->flags >> 5) != GTP_V1) 2069 | return 1; 2070 | 2071 | if (gtpv1->type != GTP_TPDU) 2072 | return 1; 2073 | 2074 | gtpv1_hdr_len = get_gtpu_header_len(gtpv1, sizeof(struct udphdr)); 2075 | if (gtpv1_hdr_len < 0) { 2076 | GTP5G_ERR(gtp->dev, "Invalid extension header length or else\n"); 2077 | return -1; 2078 | } 2079 | 2080 | hdrlen = sizeof(struct udphdr) + gtpv1_hdr_len; 2081 | if (!pskb_may_pull(skb, hdrlen)) 2082 | return -1; 2083 | 2084 | //GTP5G_ERR(gtp->dev, "Total header len(%#x)\n", hdrlen); 2085 | //gtp1 = (struct gtpv1_hdr *)(skb->data + sizeof(struct udphdr)); 2086 | pdr = pdr_find_by_gtp1u(gtp, skb, hdrlen, gtpv1->tid); 2087 | if (!pdr) { 2088 | GTP5G_ERR(gtp->dev, "No PDR match this skb : teid[%d]\n", ntohl(gtpv1->tid)); 2089 | return -1; 2090 | } 2091 | 2092 | return gtp5g_rx(pdr, skb, hdrlen, gtp->role); 2093 | } 2094 | 2095 | /** 2096 | * Entry function for Uplink packets 2097 | * 2098 | * UDP encapsulation receive handler. See net/ipv4/udp.c 2099 | * Return codes: 2100 | * =0 : if skb was successfully passed to the encap handler or 2101 | * was discarded by it 2102 | * >0 : if skb should be passed on to UDP 2103 | * <0 : if skb should be resubmitted as proto -N 2104 | */ 2105 | static int gtp5g_encap_recv(struct sock *sk, struct sk_buff *skb) 2106 | { 2107 | struct gtp5g_dev *gtp; 2108 | int ret = 0; 2109 | 2110 | gtp = rcu_dereference_sk_user_data(sk); 2111 | if (!gtp) 2112 | return 1; 2113 | 2114 | switch (udp_sk(sk)->encap_type) { 2115 | case UDP_ENCAP_GTP1U: 2116 | //GTP5G_ERR(gtp->dev, "Receive GTP-U v1 packet\n"); 2117 | ret = gtp1u_udp_encap_recv(gtp, skb); 2118 | break; 2119 | default: 2120 | ret = -1; // Should not happen 2121 | } 2122 | 2123 | switch (ret) { 2124 | case 1: 2125 | GTP5G_ERR(gtp->dev, "Pass up to the process\n"); 2126 | break; 2127 | case 0: 2128 | break; 2129 | case -1: 2130 | GTP5G_ERR(gtp->dev, "GTP packet has been dropped\n"); 2131 | kfree_skb(skb); 2132 | ret = 0; 2133 | break; 2134 | } 2135 | 2136 | return ret; 2137 | } 2138 | 2139 | static struct sock *gtp5g_encap_enable_socket(int fd, int type, struct gtp5g_dev *gtp) 2140 | { 2141 | struct udp_tunnel_sock_cfg tuncfg = {NULL}; 2142 | struct socket *sock; 2143 | struct sock *sk; 2144 | int err; 2145 | 2146 | pr_debug("enable gtp5g on %d, %d\n", fd, type); 2147 | 2148 | sock = sockfd_lookup(fd, &err); 2149 | if (!sock) { 2150 | pr_debug("gtp5g socket fd[%d] not found\n", fd); 2151 | return NULL; 2152 | } 2153 | 2154 | if (sock->sk->sk_protocol != IPPROTO_UDP) { 2155 | pr_debug("socket fd[%d] not UDP\n", fd); 2156 | sk = ERR_PTR(-EINVAL); 2157 | goto out_sock; 2158 | } 2159 | 2160 | lock_sock(sock->sk); 2161 | if (sock->sk->sk_user_data) { 2162 | sk = ERR_PTR(-EBUSY); 2163 | goto out_sock; 2164 | } 2165 | 2166 | sk = sock->sk; 2167 | sock_hold(sk); 2168 | 2169 | tuncfg.sk_user_data = gtp; 2170 | tuncfg.encap_type = type; 2171 | tuncfg.encap_rcv = gtp5g_encap_recv; 2172 | tuncfg.encap_destroy = gtp5g_encap_destroy; 2173 | 2174 | setup_udp_tunnel_sock(sock_net(sock->sk), sock, &tuncfg); 2175 | 2176 | out_sock: 2177 | release_sock(sock->sk); 2178 | sockfd_put(sock); 2179 | return sk; 2180 | } 2181 | 2182 | static int gtp5g_encap_enable(struct gtp5g_dev *gtp, struct nlattr *data[]) { 2183 | struct sock *sk = NULL; 2184 | unsigned int role = GTP5G_ROLE_UPF; 2185 | 2186 | if (data[IFLA_GTP5G_FD1]) { 2187 | u32 fd1 = nla_get_u32(data[IFLA_GTP5G_FD1]); 2188 | 2189 | sk = gtp5g_encap_enable_socket(fd1, UDP_ENCAP_GTP1U, gtp); 2190 | if (IS_ERR(sk)) 2191 | return PTR_ERR(sk); 2192 | } 2193 | 2194 | if (data[IFLA_GTP5G_ROLE]) { 2195 | role = nla_get_u32(data[IFLA_GTP5G_ROLE]); 2196 | if (role > GTP5G_ROLE_RAN) { 2197 | if (sk) 2198 | gtp5g_encap_disable_sock(sk); 2199 | return -EINVAL; 2200 | } 2201 | } 2202 | 2203 | gtp->sk1u = sk; 2204 | gtp->role = role; 2205 | 2206 | return 0; 2207 | } 2208 | 2209 | static int gtp5g_newlink(struct net *src_net, struct net_device *dev, 2210 | struct nlattr *tb[], struct nlattr *data[], 2211 | struct netlink_ext_ack *extack) 2212 | { 2213 | struct gtp5g_dev *gtp; 2214 | struct gtp5g_net *gn; 2215 | int hashsize, err; 2216 | 2217 | if (!data[IFLA_GTP5G_FD1]) 2218 | return -EINVAL; 2219 | 2220 | gtp = netdev_priv(dev); 2221 | 2222 | err = gtp5g_encap_enable(gtp, data); 2223 | if (err < 0) 2224 | return err; 2225 | 2226 | if (!data[IFLA_GTP5G_PDR_HASHSIZE]) 2227 | hashsize = 1024; 2228 | else 2229 | hashsize = nla_get_u32(data[IFLA_GTP5G_PDR_HASHSIZE]); 2230 | 2231 | err = gtp5g_hashtable_new(gtp, hashsize); 2232 | if (err < 0) 2233 | goto out_encap; 2234 | 2235 | err = register_netdevice(dev); 2236 | if (err < 0) { 2237 | GTP5G_ERR(dev, "failed to register new netdev %d\n", err); 2238 | goto out_hashtable; 2239 | } 2240 | 2241 | gn = net_generic(dev_net(dev), gtp5g_net_id); 2242 | list_add_rcu(>p->list, &gn->gtp5g_dev_list); 2243 | list_add_rcu(>p->proc_list, &proc_gtp5g_dev); 2244 | 2245 | GTP5G_ERR(dev, "registered new 5G GTP interface\n"); 2246 | 2247 | return 0; 2248 | 2249 | out_hashtable: 2250 | gtp5g_hashtable_free(gtp); 2251 | out_encap: 2252 | gtp5g_encap_disable(gtp); 2253 | return err; 2254 | 2255 | } 2256 | 2257 | static void gtp5g_dellink(struct net_device *dev, struct list_head *head) 2258 | { 2259 | struct gtp5g_dev *gtp = netdev_priv(dev); 2260 | 2261 | gtp5g_hashtable_free(gtp); 2262 | list_del_rcu(>p->list); 2263 | list_del_rcu(>p->proc_list); 2264 | unregister_netdevice_queue(dev, head); 2265 | 2266 | GTP5G_ERR(dev, "deregistered 5G GTP interface\n"); 2267 | } 2268 | 2269 | static size_t gtp5g_get_size(const struct net_device *dev) 2270 | { 2271 | return nla_total_size(sizeof(__u32)); /* IFLA_GTP5G_PDR_HASHSIZE */ 2272 | } 2273 | 2274 | static int gtp5g_fill_info(struct sk_buff *skb, const struct net_device *dev) 2275 | { 2276 | struct gtp5g_dev *gtp = netdev_priv(dev); 2277 | 2278 | if (nla_put_u32(skb, IFLA_GTP5G_PDR_HASHSIZE, gtp->hash_size)) 2279 | goto nla_put_failure; 2280 | 2281 | return 0; 2282 | 2283 | nla_put_failure: 2284 | return -EMSGSIZE; 2285 | } 2286 | 2287 | static const struct nla_policy gtp5g_policy[IFLA_GTP5G_MAX + 1] = { 2288 | [IFLA_GTP5G_FD1] = { .type = NLA_U32 }, 2289 | [IFLA_GTP5G_PDR_HASHSIZE] = { .type = NLA_U32 }, 2290 | [IFLA_GTP5G_ROLE] = { .type = NLA_U32 }, 2291 | }; 2292 | 2293 | static struct rtnl_link_ops gtp5g_link_ops __read_mostly = { 2294 | .kind = "gtp5g", 2295 | .maxtype = IFLA_GTP5G_MAX, 2296 | .policy = gtp5g_policy, 2297 | .priv_size = sizeof(struct gtp5g_dev), 2298 | .setup = gtp5g_link_setup, 2299 | .validate = gtp5g_validate, 2300 | .newlink = gtp5g_newlink, 2301 | .dellink = gtp5g_dellink, 2302 | .get_size = gtp5g_get_size, 2303 | .fill_info = gtp5g_fill_info, 2304 | }; 2305 | 2306 | static struct genl_family gtp5g_genl_family; 2307 | 2308 | static int gtp5g_gnl_add_pdr(struct gtp5g_dev *gtp, struct genl_info *info) 2309 | { 2310 | struct net_device *dev = gtp->dev; 2311 | struct gtp5g_pdr *pdr; 2312 | int err = 0; 2313 | u32 pdr_id; 2314 | 2315 | pdr_id = nla_get_u16(info->attrs[GTP5G_PDR_ID]); 2316 | pdr = pdr_find_by_id(gtp, pdr_id); 2317 | if (pdr) { 2318 | if (info->nlhdr->nlmsg_flags & NLM_F_EXCL) { 2319 | GTP5G_ERR(dev, "PDR-Add: Failed NLM_F_EXCL is set\n"); 2320 | err = -EEXIST; 2321 | goto out; 2322 | } else if (!(info->nlhdr->nlmsg_flags & NLM_F_REPLACE)) { 2323 | GTP5G_ERR(dev, "PDR-Add: Failed NLM_F_REPLACE is not set\n"); 2324 | err = -EOPNOTSUPP; 2325 | goto out; 2326 | } 2327 | 2328 | err = pdr_fill(pdr, gtp, info); 2329 | if (err < 0) { 2330 | GTP5G_ERR(dev, "PDR-Add: update id(%u) fail\n", pdr_id); 2331 | pdr_context_delete(pdr); 2332 | } 2333 | return err; 2334 | } 2335 | 2336 | if (info->nlhdr->nlmsg_flags & NLM_F_REPLACE) { 2337 | GTP5G_ERR(dev, "PDR-Add: (New)Failed NLM_F_REPLACE is set\n"); 2338 | err = -ENOENT; 2339 | goto out; 2340 | } 2341 | 2342 | if (info->nlhdr->nlmsg_flags & NLM_F_APPEND) { 2343 | GTP5G_ERR(dev, "PDR-Add: (New)Failed NLM_F_APPEND is set\n"); 2344 | err = -EOPNOTSUPP; 2345 | goto out; 2346 | } 2347 | 2348 | // Check only at the creation part 2349 | if (!info->attrs[GTP5G_PDR_PRECEDENCE]) { 2350 | GTP5G_ERR(dev, "PDR-Add: Precedence is not given\n"); 2351 | err = -EINVAL; 2352 | goto out; 2353 | } 2354 | 2355 | pdr = kzalloc(sizeof(*pdr), GFP_ATOMIC); 2356 | if (!pdr) { 2357 | GTP5G_ERR(dev, "PDR-Add: Failed to allocate memory\n"); 2358 | err = -ENOMEM; 2359 | goto out; 2360 | } 2361 | 2362 | sock_hold(gtp->sk1u); 2363 | pdr->sk = gtp->sk1u; 2364 | pdr->dev = gtp->dev; 2365 | 2366 | err = pdr_fill(pdr, gtp, info); 2367 | if (err < 0) { 2368 | GTP5G_ERR(dev, "PDR-Add: id(%u) fail: %d\n", pdr_id, err); 2369 | pdr_context_delete(pdr); 2370 | goto out; 2371 | } 2372 | 2373 | hlist_add_head_rcu(&pdr->hlist_id, 2374 | >p->pdr_id_hash[u32_hashfn(pdr_id) % gtp->hash_size]); 2375 | GTP5G_ERR(dev, "PDR-Add: id[%d] success\n", pdr_id); 2376 | 2377 | out: 2378 | return err; 2379 | } 2380 | 2381 | static int gtp5g_genl_add_pdr(struct sk_buff *skb, struct genl_info *info) 2382 | { 2383 | struct gtp5g_dev *gtp; 2384 | int err = 0; 2385 | 2386 | if (!info->attrs[GTP5G_PDR_ID] || 2387 | !info->attrs[GTP5G_LINK]) { 2388 | GTP5G_ERR(NULL, "PDR-Add: ID or LINK value is not exists\n"); 2389 | return -EINVAL; 2390 | } 2391 | 2392 | rtnl_lock(); 2393 | rcu_read_lock(); 2394 | 2395 | gtp = gtp5g_find_dev(sock_net(skb->sk), info->attrs); 2396 | if (!gtp) { 2397 | GTP5G_ERR(NULL, "PDR-Add: Can't find the gtp5g_dev\n"); 2398 | err = -ENODEV; 2399 | goto unlock; 2400 | } 2401 | 2402 | err = gtp5g_gnl_add_pdr(gtp, info); 2403 | 2404 | unlock: 2405 | rcu_read_unlock(); 2406 | rtnl_unlock(); 2407 | return err; 2408 | } 2409 | 2410 | static int gtp5g_genl_del_pdr(struct sk_buff *skb, struct genl_info *info) 2411 | { 2412 | u16 id; 2413 | struct gtp5g_pdr *pdr; 2414 | int err = 0; 2415 | 2416 | if (!info->attrs[GTP5G_PDR_ID] || 2417 | !info->attrs[GTP5G_LINK]) { 2418 | GTP5G_ERR(NULL, "PDR-Del: ID or LINK is not present\n"); 2419 | return -EINVAL; 2420 | } 2421 | 2422 | id = nla_get_u16(info->attrs[GTP5G_PDR_ID]); 2423 | 2424 | rcu_read_lock(); 2425 | 2426 | pdr = gtp5g_find_pdr(sock_net(skb->sk), info->attrs); 2427 | if (IS_ERR(pdr)) { 2428 | GTP5G_ERR(NULL, "PDR-Del: failed to find id(%#x)\n", id); 2429 | err = PTR_ERR(pdr); 2430 | goto unlock; 2431 | } 2432 | 2433 | GTP5G_LOG(pdr->dev, "PDR-Del: id[%d] success\n", id); 2434 | pdr_context_delete(pdr); 2435 | 2436 | unlock: 2437 | rcu_read_unlock(); 2438 | return err; 2439 | } 2440 | 2441 | static int gtp5g_genl_fill_pdr(struct sk_buff *skb, u32 snd_portid, u32 snd_seq, 2442 | u32 type, struct gtp5g_pdr *pdr) 2443 | { 2444 | void *genlh; 2445 | struct nlattr *nest_pdi, *nest_f_teid, *nest_sdf, *nest_rule; 2446 | struct gtp5g_pdi *pdi; 2447 | struct local_f_teid *f_teid; 2448 | struct sdf_filter *sdf; 2449 | struct ip_filter_rule *rule; 2450 | 2451 | int i; 2452 | u32 *u32_buf = kzalloc(0xff * sizeof(u32), GFP_KERNEL); 2453 | if (!u32_buf) { 2454 | GTP5G_ERR(NULL, "Failed to allocate memory\n"); 2455 | goto out; 2456 | } 2457 | 2458 | genlh = genlmsg_put(skb, snd_portid, snd_seq, >p5g_genl_family, 0, type); 2459 | if (!genlh) 2460 | goto genlmsg_fail; 2461 | 2462 | if (nla_put_u16(skb, GTP5G_PDR_ID, pdr->id) || 2463 | nla_put_u32(skb, GTP5G_PDR_PRECEDENCE, pdr->precedence)) 2464 | goto genlmsg_fail; 2465 | 2466 | if (pdr->outer_header_removal) { 2467 | if (nla_put_u8(skb, GTP5G_OUTER_HEADER_REMOVAL, *pdr->outer_header_removal)) 2468 | goto genlmsg_fail; 2469 | } 2470 | 2471 | if (pdr->far_id) { 2472 | if (nla_put_u32(skb, GTP5G_PDR_FAR_ID, *pdr->far_id)) 2473 | goto genlmsg_fail; 2474 | } 2475 | 2476 | if (pdr->qer_id) { 2477 | if (nla_put_u32(skb, GTP5G_PDR_QER_ID, *pdr->qer_id)) 2478 | goto genlmsg_fail; 2479 | } 2480 | 2481 | if (pdr->role_addr_ipv4.s_addr) { 2482 | if (nla_put_u32(skb, GTP5G_PDR_ROLE_ADDR_IPV4, pdr->role_addr_ipv4.s_addr)) 2483 | goto genlmsg_fail; 2484 | } 2485 | 2486 | if (pdr->pdi) { 2487 | if (!(nest_pdi = nla_nest_start(skb, GTP5G_PDR_PDI))) 2488 | goto genlmsg_fail; 2489 | 2490 | pdi = pdr->pdi; 2491 | if (pdi->ue_addr_ipv4) { 2492 | if (nla_put_be32(skb, GTP5G_PDI_UE_ADDR_IPV4, pdi->ue_addr_ipv4->s_addr)) 2493 | goto genlmsg_fail; 2494 | } 2495 | 2496 | if (pdi->f_teid) { 2497 | if (!(nest_f_teid = nla_nest_start(skb, GTP5G_PDI_F_TEID))) 2498 | goto genlmsg_fail; 2499 | 2500 | f_teid = pdi->f_teid; 2501 | if (nla_put_u32(skb, GTP5G_F_TEID_I_TEID, ntohl(f_teid->teid)) || 2502 | nla_put_be32(skb, GTP5G_F_TEID_GTPU_ADDR_IPV4, f_teid->gtpu_addr_ipv4.s_addr)) 2503 | goto genlmsg_fail; 2504 | 2505 | nla_nest_end(skb, nest_f_teid); 2506 | } 2507 | 2508 | if (pdi->sdf) { 2509 | if (!(nest_sdf = nla_nest_start(skb, GTP5G_PDI_SDF_FILTER))) 2510 | goto genlmsg_fail; 2511 | 2512 | sdf = pdi->sdf; 2513 | if (sdf->rule) { 2514 | if (!(nest_rule = nla_nest_start(skb, GTP5G_SDF_FILTER_FLOW_DESCRIPTION))) 2515 | goto genlmsg_fail; 2516 | rule = sdf->rule; 2517 | 2518 | if (nla_put_u8(skb, GTP5G_FLOW_DESCRIPTION_ACTION, rule->action) || 2519 | nla_put_u8(skb, GTP5G_FLOW_DESCRIPTION_DIRECTION, rule->direction) || 2520 | nla_put_u8(skb, GTP5G_FLOW_DESCRIPTION_PROTOCOL, rule->proto) || 2521 | nla_put_be32(skb, GTP5G_FLOW_DESCRIPTION_SRC_IPV4, rule->src.s_addr) || 2522 | nla_put_be32(skb, GTP5G_FLOW_DESCRIPTION_DEST_IPV4, rule->dest.s_addr)) 2523 | goto genlmsg_fail; 2524 | 2525 | if (rule->smask.s_addr != -1) 2526 | if (nla_put_be32(skb, GTP5G_FLOW_DESCRIPTION_SRC_MASK, rule->smask.s_addr)) 2527 | goto genlmsg_fail; 2528 | 2529 | if (rule->dmask.s_addr != -1) 2530 | if (nla_put_be32(skb, GTP5G_FLOW_DESCRIPTION_DEST_MASK, rule->dmask.s_addr)) 2531 | goto genlmsg_fail; 2532 | 2533 | if (rule->sport_num && rule->sport) { 2534 | for (i = 0; i < rule->sport_num; i++) 2535 | u32_buf[i] = rule->sport[i].start + (rule->sport[i].end << 16); 2536 | if (nla_put(skb, GTP5G_FLOW_DESCRIPTION_SRC_PORT, 2537 | rule->sport_num * sizeof(u32) / sizeof(char), u32_buf)) 2538 | goto genlmsg_fail; 2539 | } 2540 | 2541 | if (rule->dport_num && rule->dport) { 2542 | for (i = 0; i < rule->dport_num; i++) 2543 | u32_buf[i] = rule->dport[i].start + (rule->dport[i].end << 16); 2544 | if (nla_put(skb, GTP5G_FLOW_DESCRIPTION_DEST_PORT, 2545 | rule->dport_num * sizeof(u32) / sizeof(char), u32_buf)) 2546 | goto genlmsg_fail; 2547 | } 2548 | 2549 | nla_nest_end(skb, nest_rule); 2550 | } 2551 | 2552 | if (sdf->tos_traffic_class) 2553 | if (nla_put_u16(skb, GTP5G_SDF_FILTER_TOS_TRAFFIC_CLASS, *sdf->tos_traffic_class)) 2554 | goto genlmsg_fail; 2555 | 2556 | if (sdf->security_param_idx) 2557 | if (nla_put_u32(skb, GTP5G_SDF_FILTER_SECURITY_PARAMETER_INDEX, *sdf->security_param_idx)) 2558 | goto genlmsg_fail; 2559 | 2560 | if (sdf->flow_label) 2561 | if (nla_put_u32(skb, GTP5G_SDF_FILTER_FLOW_LABEL, *sdf->flow_label)) 2562 | goto genlmsg_fail; 2563 | 2564 | if (sdf->bi_id) 2565 | if (nla_put_u32(skb, GTP5G_SDF_FILTER_SDF_FILTER_ID, *sdf->bi_id)) 2566 | goto genlmsg_fail; 2567 | 2568 | nla_nest_end(skb, nest_sdf); 2569 | } 2570 | 2571 | nla_nest_end(skb, nest_pdi); 2572 | } 2573 | 2574 | kfree(u32_buf); 2575 | genlmsg_end(skb, genlh); 2576 | 2577 | return 0; 2578 | 2579 | genlmsg_fail: 2580 | kfree(u32_buf); 2581 | genlmsg_cancel(skb, genlh); 2582 | out: 2583 | return -EMSGSIZE; 2584 | } 2585 | 2586 | static int gtp5g_genl_get_pdr(struct sk_buff *skb, struct genl_info *info) 2587 | { 2588 | struct gtp5g_pdr *pdr; 2589 | struct sk_buff *skb_ack; 2590 | int err; 2591 | 2592 | if (!info->attrs[GTP5G_PDR_ID]) { 2593 | GTP5G_ERR(NULL, "PDR ID is not given\n"); 2594 | return -EINVAL; 2595 | } 2596 | 2597 | rcu_read_lock(); 2598 | pdr = gtp5g_find_pdr(sock_net(skb->sk), info->attrs); 2599 | if (IS_ERR(pdr)) { 2600 | GTP5G_ERR(NULL, "PDR record is not present\n"); 2601 | err = PTR_ERR(pdr); 2602 | goto unlock; 2603 | } 2604 | 2605 | skb_ack = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC); 2606 | if (!skb_ack) { 2607 | GTP5G_ERR(NULL, "Failed to allocate skb ack\n"); 2608 | err = -ENOMEM; 2609 | goto unlock; 2610 | } 2611 | 2612 | err = gtp5g_genl_fill_pdr(skb_ack, 2613 | NETLINK_CB(skb).portid, 2614 | info->snd_seq, 2615 | info->nlhdr->nlmsg_type, 2616 | pdr); 2617 | if (err < 0) { 2618 | GTP5G_ERR(NULL, "Failed to fill PDR err(%d)\n", err); 2619 | goto freebuf; 2620 | } 2621 | rcu_read_unlock(); 2622 | return genlmsg_unicast(genl_info_net(info), skb_ack, info->snd_portid); 2623 | 2624 | freebuf: 2625 | kfree_skb(skb_ack); 2626 | unlock: 2627 | rcu_read_unlock(); 2628 | return err; 2629 | } 2630 | 2631 | static int gtp5g_genl_dump_pdr(struct sk_buff *skb, struct netlink_callback *cb) 2632 | { 2633 | /* netlink_callback->args 2634 | * args[0] : index of gtp5g dev id 2635 | * args[1] : index of gtp5g hash entry id in dev 2636 | * args[2] : index of gtp5g pdr id 2637 | * args[5] : set non-zero means it is finished 2638 | */ 2639 | struct gtp5g_dev *gtp, *last_gtp = (struct gtp5g_dev *)cb->args[0]; 2640 | struct net *net = sock_net(skb->sk); 2641 | struct gtp5g_net *gn = net_generic(net, gtp5g_net_id); 2642 | int i, last_hash_entry_id = cb->args[1], ret; 2643 | u16 pdr_id = cb->args[2]; 2644 | struct gtp5g_pdr *pdr; 2645 | 2646 | if (cb->args[5]) { 2647 | GTP5G_ERR(NULL, "Failed to dump callback args[5] is present\n"); 2648 | return 0; 2649 | } 2650 | 2651 | list_for_each_entry_rcu(gtp, &gn->gtp5g_dev_list, list) { 2652 | if (last_gtp && last_gtp != gtp) 2653 | continue; 2654 | else 2655 | last_gtp = NULL; 2656 | 2657 | for (i = last_hash_entry_id; i < gtp->hash_size; i++) { 2658 | hlist_for_each_entry_rcu(pdr, >p->pdr_id_hash[i], hlist_id) { 2659 | if (pdr_id && pdr_id != pdr->id) 2660 | continue; 2661 | else 2662 | pdr_id = 0; 2663 | 2664 | ret = gtp5g_genl_fill_pdr(skb, 2665 | NETLINK_CB(cb->skb).portid, 2666 | cb->nlh->nlmsg_seq, 2667 | cb->nlh->nlmsg_type, 2668 | pdr); 2669 | if (ret < 0) { 2670 | cb->args[0] = (unsigned long) gtp; 2671 | cb->args[1] = i; 2672 | cb->args[2] = pdr->id; 2673 | goto out; 2674 | } 2675 | } 2676 | } 2677 | } 2678 | cb->args[5] = 1; 2679 | 2680 | out: 2681 | return skb->len; 2682 | } 2683 | 2684 | static int gtp5g_gnl_add_far(struct gtp5g_dev *gtp, struct genl_info *info) 2685 | { 2686 | struct net_device *dev = gtp->dev; 2687 | struct gtp5g_far *far; 2688 | struct gtp5g_emark_pktinfo epkt_info; 2689 | int err = 0; 2690 | u32 far_id; 2691 | u8 flag; 2692 | 2693 | far_id = nla_get_u32(info->attrs[GTP5G_FAR_ID]); 2694 | far = far_find_by_id(gtp, far_id); 2695 | if (far) { 2696 | if (info->nlhdr->nlmsg_flags & NLM_F_EXCL) { 2697 | GTP5G_ERR(dev, "FAR-Add: Failed NLM_F_EXCL is set\n"); 2698 | err = -EEXIST; 2699 | goto out; 2700 | } else if (!(info->nlhdr->nlmsg_flags & NLM_F_REPLACE)) { 2701 | GTP5G_ERR(dev, "FAR-Add: Failed NLM_F_REPLACE is not set\n"); 2702 | err = -EOPNOTSUPP; 2703 | goto out; 2704 | } 2705 | 2706 | flag = 0; 2707 | err = far_fill(far, gtp, info, &flag, &epkt_info); 2708 | if (err < 0) { 2709 | far_context_delete(far); 2710 | GTP5G_ERR(dev,"FAR-Add: update id[%d] fail: %d\n", far_id, err); 2711 | goto out; 2712 | } 2713 | 2714 | // Send GTP-U End marker to gNB 2715 | if (flag) { 2716 | /* SKB size GTPU(8) + UDP(8) + IP(20) + Eth(14) 2717 | * + 2-Bytes align the IP header 2718 | * */ 2719 | struct sk_buff *skb = __netdev_alloc_skb(dev, 52, GFP_KERNEL); 2720 | if (!skb) { 2721 | GTP5G_ERR(dev, "FAR-Add: Failled to allocate skb with a size 52\n"); 2722 | err = 0; 2723 | goto out; 2724 | } 2725 | skb_reserve(skb, 2); 2726 | skb->protocol = eth_type_trans(skb, dev); 2727 | gtp5g_fwd_emark_skb_ipv4(skb, dev, &epkt_info); 2728 | } 2729 | GTP5G_LOG(dev, "FAR-Add: update id[%d] success\n", far_id); 2730 | return 0; 2731 | } 2732 | 2733 | if (info->nlhdr->nlmsg_flags & NLM_F_REPLACE) { 2734 | GTP5G_ERR(dev, "FAR-Add: Failed NLM_F_REPLACE is set\n"); 2735 | err = -ENOENT; 2736 | goto out; 2737 | } 2738 | 2739 | if (info->nlhdr->nlmsg_flags & NLM_F_APPEND) { 2740 | GTP5G_ERR(dev, "FAR-Add: Failed NLM_F_APPEND is set\n"); 2741 | err = -EOPNOTSUPP; 2742 | goto out; 2743 | } 2744 | 2745 | // Check only at the creation part 2746 | if (!info->attrs[GTP5G_FAR_APPLY_ACTION]) { 2747 | GTP5G_ERR(dev, "FAR-Add: Failed apply action is not given\n"); 2748 | err = -EINVAL; 2749 | goto out; 2750 | } 2751 | 2752 | far = kzalloc(sizeof(*far), GFP_ATOMIC); 2753 | if (!far) { 2754 | GTP5G_ERR(dev, "FAR-Add: Failed to allocate memory\n"); 2755 | err = -ENOMEM; 2756 | goto out; 2757 | } 2758 | far->dev = gtp->dev; 2759 | 2760 | err = far_fill(far, gtp, info, NULL, NULL); 2761 | if (err < 0) { 2762 | GTP5G_ERR(dev, "FAR-Add: id[%d] fail\n", far_id); 2763 | far_context_delete(far); 2764 | goto out; 2765 | } 2766 | 2767 | hlist_add_head_rcu(&far->hlist_id, 2768 | >p->far_id_hash[u32_hashfn(far_id) % gtp->hash_size]); 2769 | GTP5G_LOG(dev, "FAR-Add: id[%d] success\n", far_id); 2770 | 2771 | out: 2772 | return err; 2773 | } 2774 | 2775 | static int gtp5g_genl_add_far(struct sk_buff *skb, struct genl_info *info) 2776 | { 2777 | struct gtp5g_dev *gtp; 2778 | int err = 0; 2779 | 2780 | if (!info->attrs[GTP5G_FAR_ID] || 2781 | !info->attrs[GTP5G_LINK]) { 2782 | GTP5G_ERR(NULL, "Failed to find FAR_ID or LINK in netlink\n"); 2783 | return -EINVAL; 2784 | } 2785 | 2786 | rtnl_lock(); 2787 | rcu_read_lock(); 2788 | 2789 | gtp = gtp5g_find_dev(sock_net(skb->sk), info->attrs); 2790 | if (!gtp) { 2791 | GTP5G_ERR(NULL, "Failed to find the gtp5g_dev\n"); 2792 | err = -ENODEV; 2793 | goto unlock; 2794 | } 2795 | 2796 | err = gtp5g_gnl_add_far(gtp, info); 2797 | 2798 | unlock: 2799 | rcu_read_unlock(); 2800 | rtnl_unlock(); 2801 | return err; 2802 | } 2803 | 2804 | static int gtp5g_genl_del_far(struct sk_buff *skb, struct genl_info *info) 2805 | { 2806 | u32 id; 2807 | struct gtp5g_far *far; 2808 | int err = 0; 2809 | 2810 | if (!info->attrs[GTP5G_FAR_ID] || 2811 | !info->attrs[GTP5G_LINK]) { 2812 | GTP5G_ERR(NULL, "Failed to find FAR_ID or LINK in netlink\n"); 2813 | return -EINVAL; 2814 | } 2815 | 2816 | id = nla_get_u32(info->attrs[GTP5G_FAR_ID]); 2817 | 2818 | rcu_read_lock(); 2819 | 2820 | far = gtp5g_find_far(sock_net(skb->sk), info->attrs); 2821 | if (IS_ERR(far)) { 2822 | GTP5G_ERR(NULL, "Failed to find far\n"); 2823 | err = PTR_ERR(far); 2824 | goto unlock; 2825 | } 2826 | 2827 | GTP5G_ERR(far->dev, "5G GTP-U : delete FAR id(%u)\n", id); 2828 | far_context_delete(far); 2829 | 2830 | unlock: 2831 | rcu_read_unlock(); 2832 | return err; 2833 | } 2834 | 2835 | static int gtp5g_genl_fill_far(struct sk_buff *skb, u32 snd_portid, u32 snd_seq, 2836 | u32 type, struct gtp5g_far *far) 2837 | { 2838 | void *genlh; 2839 | struct nlattr *nest_fwd_param, *nest_hdr_creation; 2840 | struct forwarding_parameter *fwd_param; 2841 | struct outer_header_creation *hdr_creation; 2842 | struct forwarding_policy *fwd_policy; 2843 | 2844 | int cnt; 2845 | struct gtp5g_dev *gtp = netdev_priv(far->dev); 2846 | struct hlist_head *head; 2847 | struct gtp5g_pdr *pdr; 2848 | u16 *u16_buf = kzalloc(0xff * sizeof(u16), GFP_KERNEL); 2849 | if (!u16_buf) { 2850 | GTP5G_ERR(NULL, "Failed to allocate buf\n"); 2851 | goto out; 2852 | } 2853 | 2854 | genlh = genlmsg_put(skb, snd_portid, snd_seq, >p5g_genl_family, 0, type); 2855 | if (!genlh) 2856 | goto genlmsg_fail; 2857 | 2858 | if (nla_put_u32(skb, GTP5G_FAR_ID, far->id) || 2859 | nla_put_u8(skb, GTP5G_FAR_APPLY_ACTION, far->action)) 2860 | goto genlmsg_fail; 2861 | 2862 | if (far->fwd_param) { 2863 | if (!(nest_fwd_param = nla_nest_start(skb, GTP5G_FAR_FORWARDING_PARAMETER))) 2864 | goto genlmsg_fail; 2865 | 2866 | fwd_param = far->fwd_param; 2867 | if (fwd_param->hdr_creation) { 2868 | if (!(nest_hdr_creation = nla_nest_start(skb, GTP5G_FORWARDING_PARAMETER_OUTER_HEADER_CREATION))) 2869 | goto genlmsg_fail; 2870 | 2871 | hdr_creation = fwd_param->hdr_creation; 2872 | if (nla_put_u16(skb, GTP5G_OUTER_HEADER_CREATION_DESCRIPTION, hdr_creation->description) || 2873 | nla_put_u32(skb, GTP5G_OUTER_HEADER_CREATION_O_TEID, ntohl(hdr_creation->teid)) || 2874 | nla_put_be32(skb, GTP5G_OUTER_HEADER_CREATION_PEER_ADDR_IPV4, hdr_creation->peer_addr_ipv4.s_addr) || 2875 | nla_put_u16(skb, GTP5G_OUTER_HEADER_CREATION_PORT, ntohs(hdr_creation->port))) 2876 | goto genlmsg_fail; 2877 | 2878 | nla_nest_end(skb, nest_hdr_creation); 2879 | } 2880 | 2881 | if ((fwd_policy = fwd_param->fwd_policy)) 2882 | if (nla_put(skb, GTP5G_FORWARDING_PARAMETER_FORWARDING_POLICY, fwd_policy->len, fwd_policy->identifier)) 2883 | goto genlmsg_fail; 2884 | 2885 | nla_nest_end(skb, nest_fwd_param); 2886 | } 2887 | 2888 | cnt = 0; 2889 | head = >p->related_far_hash[u32_hashfn(far->id) % gtp->hash_size]; 2890 | hlist_for_each_entry_rcu(pdr, head, hlist_related_far) { 2891 | if (cnt >= 0xff) 2892 | goto genlmsg_fail; 2893 | 2894 | if (*pdr->far_id == far->id) 2895 | u16_buf[cnt++] = pdr->id; 2896 | } 2897 | 2898 | if (cnt) { 2899 | if (nla_put(skb, GTP5G_FAR_RELATED_TO_PDR, cnt * sizeof(u16) / sizeof(char), u16_buf)) 2900 | goto genlmsg_fail; 2901 | } 2902 | 2903 | kfree(u16_buf); 2904 | genlmsg_end(skb, genlh); 2905 | return 0; 2906 | 2907 | genlmsg_fail: 2908 | kfree(u16_buf); 2909 | genlmsg_cancel(skb, genlh); 2910 | out: 2911 | return -EMSGSIZE; 2912 | } 2913 | 2914 | static int gtp5g_genl_get_far(struct sk_buff *skb, struct genl_info *info) 2915 | { 2916 | struct gtp5g_far *far; 2917 | struct sk_buff *skb_ack; 2918 | int err; 2919 | 2920 | if (!info->attrs[GTP5G_FAR_ID]) { 2921 | GTP5G_ERR(NULL, "Failed to find FAR_ID in netlink msg\n"); 2922 | return -EINVAL; 2923 | } 2924 | 2925 | rcu_read_lock(); 2926 | 2927 | far = gtp5g_find_far(sock_net(skb->sk), info->attrs); 2928 | if (IS_ERR(far)) { 2929 | GTP5G_ERR(NULL, "Failed to find far\n"); 2930 | err = PTR_ERR(far); 2931 | goto unlock; 2932 | } 2933 | 2934 | skb_ack = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC); 2935 | if (!skb_ack) { 2936 | GTP5G_ERR(NULL, "Failed to allocate skb_ack\n"); 2937 | err = -ENOMEM; 2938 | goto unlock; 2939 | } 2940 | 2941 | err = gtp5g_genl_fill_far(skb_ack, 2942 | NETLINK_CB(skb).portid, 2943 | info->snd_seq, 2944 | info->nlhdr->nlmsg_type, 2945 | far); 2946 | if (err < 0) { 2947 | GTP5G_ERR(NULL, "Failed to fill far\n"); 2948 | goto freebuf; 2949 | } 2950 | 2951 | rcu_read_unlock(); 2952 | 2953 | return genlmsg_unicast(genl_info_net(info), skb_ack, info->snd_portid); 2954 | 2955 | freebuf: 2956 | kfree_skb(skb_ack); 2957 | unlock: 2958 | rcu_read_unlock(); 2959 | return err; 2960 | } 2961 | 2962 | static int gtp5g_genl_dump_far(struct sk_buff *skb, struct netlink_callback *cb) 2963 | { 2964 | /* netlink_callback->args 2965 | * args[0] : index of gtp5g dev id 2966 | * args[1] : index of gtp5g hash entry id in dev 2967 | * args[2] : index of gtp5g far id 2968 | * args[5] : set non-zero means it is finished 2969 | */ 2970 | struct gtp5g_dev *gtp, *last_gtp = (struct gtp5g_dev *)cb->args[0]; 2971 | struct net *net = sock_net(skb->sk); 2972 | struct gtp5g_net *gn = net_generic(net, gtp5g_net_id); 2973 | int i, last_hash_entry_id = cb->args[1], ret; 2974 | u32 far_id = cb->args[2]; 2975 | struct gtp5g_far *far; 2976 | 2977 | if (cb->args[5]) { 2978 | GTP5G_ERR(NULL, "Failed to dump FAR arg5 present\n"); 2979 | return 0; 2980 | } 2981 | 2982 | list_for_each_entry_rcu(gtp, &gn->gtp5g_dev_list, list) { 2983 | if (last_gtp && last_gtp != gtp) 2984 | continue; 2985 | else 2986 | last_gtp = NULL; 2987 | 2988 | for (i = last_hash_entry_id; i < gtp->hash_size; i++) { 2989 | hlist_for_each_entry_rcu(far, >p->far_id_hash[i], hlist_id) { 2990 | if (far_id && far_id != far->id) 2991 | continue; 2992 | else 2993 | far_id = 0; 2994 | 2995 | ret = gtp5g_genl_fill_far(skb, 2996 | NETLINK_CB(cb->skb).portid, 2997 | cb->nlh->nlmsg_seq, 2998 | cb->nlh->nlmsg_type, 2999 | far); 3000 | if (ret < 0) { 3001 | cb->args[0] = (unsigned long) gtp; 3002 | cb->args[1] = i; 3003 | cb->args[2] = far->id; 3004 | goto out; 3005 | } 3006 | } 3007 | } 3008 | } 3009 | cb->args[5] = 1; 3010 | 3011 | out: 3012 | return skb->len; 3013 | } 3014 | 3015 | /** --------------------------------------------------------------------- 3016 | * QER 3017 | * --------------------------------------------------------------------- 3018 | * */ 3019 | static void qer_context_free(struct rcu_head *head) 3020 | { 3021 | struct gtp5g_qer *qer = container_of(head, struct gtp5g_qer, rcu_head); 3022 | 3023 | if (!qer) 3024 | return; 3025 | 3026 | kfree(qer); 3027 | } 3028 | 3029 | static void qer_context_delete(struct gtp5g_qer *qer) 3030 | { 3031 | struct gtp5g_dev *gtp = netdev_priv(qer->dev); 3032 | struct hlist_head *head; 3033 | struct gtp5g_pdr *pdr; 3034 | 3035 | if (!qer) 3036 | return; 3037 | 3038 | if (!hlist_unhashed(&qer->hlist_id)) 3039 | hlist_del_rcu(&qer->hlist_id); 3040 | 3041 | head = >p->related_qer_hash[u32_hashfn(qer->id) % gtp->hash_size]; 3042 | hlist_for_each_entry_rcu(pdr, head, hlist_related_qer) { 3043 | if (*pdr->qer_id == qer->id) { 3044 | pdr->qer = NULL; 3045 | unix_sock_client_delete(pdr); 3046 | } 3047 | } 3048 | 3049 | call_rcu(&qer->rcu_head, qer_context_free); 3050 | } 3051 | 3052 | static int qer_fill(struct gtp5g_qer *qer, struct gtp5g_dev *gtp, struct genl_info *info) 3053 | { 3054 | struct nlattr *mbr_param_attrs[GTP5G_QER_MBR_ATTR_MAX + 1]; 3055 | struct nlattr *gbr_param_attrs[GTP5G_QER_GBR_ATTR_MAX + 1]; 3056 | struct gtp5g_pdr *pdr; 3057 | struct hlist_head *head; 3058 | 3059 | qer->id = nla_get_u32(info->attrs[GTP5G_QER_ID]); 3060 | 3061 | if (info->attrs[GTP5G_QER_GATE]) { 3062 | qer->ul_dl_gate = nla_get_u8(info->attrs[GTP5G_QER_GATE]); 3063 | } 3064 | 3065 | /* MBR */ 3066 | if (info->attrs[GTP5G_QER_MBR] && 3067 | !nla_parse_nested(mbr_param_attrs, GTP5G_QER_MBR_ATTR_MAX, info->attrs[GTP5G_QER_MBR], NULL, NULL)) { 3068 | qer->mbr.ul_high = nla_get_u32(mbr_param_attrs[GTP5G_QER_MBR_UL_HIGH32]); 3069 | qer->mbr.ul_low = nla_get_u8(mbr_param_attrs[GTP5G_QER_MBR_UL_LOW8]); 3070 | qer->mbr.dl_high = nla_get_u32(mbr_param_attrs[GTP5G_QER_MBR_DL_HIGH32]); 3071 | qer->mbr.dl_low = nla_get_u8(mbr_param_attrs[GTP5G_QER_MBR_DL_LOW8]); 3072 | } 3073 | 3074 | /* GBR */ 3075 | if (info->attrs[GTP5G_QER_GBR] && 3076 | !nla_parse_nested(gbr_param_attrs, GTP5G_QER_GBR_ATTR_MAX, info->attrs[GTP5G_QER_GBR], NULL, NULL)) { 3077 | qer->gbr.ul_high = nla_get_u32(gbr_param_attrs[GTP5G_QER_GBR_UL_HIGH32]); 3078 | qer->gbr.ul_low = nla_get_u8(gbr_param_attrs[GTP5G_QER_GBR_UL_LOW8]); 3079 | qer->gbr.dl_high = nla_get_u32(gbr_param_attrs[GTP5G_QER_GBR_DL_HIGH32]); 3080 | qer->gbr.dl_low = nla_get_u8(gbr_param_attrs[GTP5G_QER_GBR_DL_LOW8]); 3081 | } 3082 | 3083 | if (info->attrs[GTP5G_QER_CORR_ID]) { 3084 | qer->qer_corr_id = nla_get_u32(info->attrs[GTP5G_QER_CORR_ID]); 3085 | } 3086 | 3087 | if (info->attrs[GTP5G_QER_RQI]) { 3088 | qer->rqi = nla_get_u8(info->attrs[GTP5G_QER_RQI]); 3089 | } 3090 | 3091 | if (info->attrs[GTP5G_QER_QFI]) { 3092 | qer->qfi = nla_get_u8(info->attrs[GTP5G_QER_QFI]); 3093 | } 3094 | 3095 | if (info->attrs[GTP5G_QER_PPI]) { 3096 | qer->ppi = nla_get_u8(info->attrs[GTP5G_QER_PPI]); 3097 | } 3098 | 3099 | if (info->attrs[GTP5G_QER_RCSR]) { 3100 | qer->rcsr = nla_get_u8(info->attrs[GTP5G_QER_RCSR]); 3101 | } 3102 | 3103 | /* Update PDRs which has not linked to this QER */ 3104 | head = >p->related_qer_hash[u32_hashfn(qer->id) % gtp->hash_size]; 3105 | hlist_for_each_entry_rcu(pdr, head, hlist_related_qer) { 3106 | if (*pdr->qer_id == qer->id) { 3107 | pdr->qer = qer; 3108 | if (unix_sock_client_update(pdr) < 0) 3109 | GTP5G_ERR(NULL, "PDR[%u] update fail when QER[%u] apply action is changed\n", 3110 | pdr->id, qer->id); 3111 | } 3112 | } 3113 | 3114 | return 0; 3115 | } 3116 | 3117 | static struct gtp5g_qer *qer_find_by_id(struct gtp5g_dev *gtp, u32 id) 3118 | { 3119 | struct hlist_head *head; 3120 | struct gtp5g_qer *qer; 3121 | 3122 | head = >p->qer_id_hash[u32_hashfn(id) % gtp->hash_size]; 3123 | hlist_for_each_entry_rcu(qer, head, hlist_id) { 3124 | if (qer->id == id) 3125 | return qer; 3126 | } 3127 | 3128 | return NULL; 3129 | } 3130 | 3131 | static int gtp5g_gnl_add_qer(struct gtp5g_dev *gtp, struct genl_info *info) 3132 | { 3133 | 3134 | struct net_device *dev = gtp->dev; 3135 | struct gtp5g_qer *qer; 3136 | int err = 0; 3137 | u32 qer_id; 3138 | 3139 | if (!dev) { 3140 | GTP5G_ERR(NULL, "Object net_device not found\n"); 3141 | return -EEXIST; 3142 | } 3143 | 3144 | qer_id = nla_get_u32(info->attrs[GTP5G_QER_ID]); 3145 | qer = qer_find_by_id(gtp, qer_id); 3146 | if (qer) { 3147 | if (info->nlhdr->nlmsg_flags & NLM_F_EXCL) { 3148 | GTP5G_ERR(NULL, "QER-Add: Failed NLM_F_EXCL is set\n"); 3149 | return -EEXIST; 3150 | } else if (!(info->nlhdr->nlmsg_flags & NLM_F_REPLACE)) { 3151 | GTP5G_ERR(NULL, "QER-Add: Failed NLM_F_REPLACE is not set\n"); 3152 | return -EOPNOTSUPP; 3153 | } 3154 | 3155 | err = qer_fill(qer, gtp, info); 3156 | if (err < 0) { 3157 | qer_context_delete(qer); 3158 | GTP5G_ERR(dev, "QER-Add: update QER_ID(%u) err(%u)\n", qer_id, err); 3159 | return err; 3160 | } 3161 | GTP5G_LOG(dev, "QER-Add: updated QER_ID(%u)\n", qer_id); 3162 | return err; 3163 | } 3164 | 3165 | if (info->nlhdr->nlmsg_flags & NLM_F_REPLACE) { 3166 | GTP5G_ERR(dev, "QER-Add: Invalid flage set NLM_F_REPLACE\n"); 3167 | return -ENOENT; 3168 | } 3169 | 3170 | if (info->nlhdr->nlmsg_flags & NLM_F_APPEND) { 3171 | GTP5G_ERR(dev, "QER-Add: Invalid flage set NLM_F_APPEND\n"); 3172 | return -EOPNOTSUPP; 3173 | } 3174 | 3175 | qer = kzalloc(sizeof(*qer), GFP_ATOMIC); 3176 | if (!qer) { 3177 | GTP5G_ERR(dev, "QER-Add: Failed to allocate memory for QER_ID(%u)\n", qer_id); 3178 | return -ENOMEM; 3179 | } 3180 | 3181 | qer->dev = gtp->dev; 3182 | err = qer_fill(qer, gtp, info); 3183 | if (err < 0) { 3184 | GTP5G_ERR(dev, "QER-Add: QER_ID(%u) fail\n", qer_id); 3185 | qer_context_delete(qer); 3186 | goto out; 3187 | } 3188 | 3189 | hlist_add_head_rcu(&qer->hlist_id, 3190 | >p->qer_id_hash[u32_hashfn(qer_id) % gtp->hash_size]); 3191 | GTP5G_ERR(dev, "QER-Add: QER_ID(%u) success\n", qer_id); 3192 | out: 3193 | return err; 3194 | } 3195 | 3196 | 3197 | static int gtp5g_genl_add_qer(struct sk_buff *skb, struct genl_info *info) 3198 | { 3199 | struct gtp5g_dev *gtp; 3200 | int err = 0; 3201 | 3202 | if (!info->attrs[GTP5G_QER_ID] || 3203 | !info->attrs[GTP5G_LINK]) { 3204 | GTP5G_ERR(NULL, "QER_ID or GTP5g_LINK is not present\n"); 3205 | return -EINVAL; 3206 | } 3207 | 3208 | rtnl_lock(); 3209 | rcu_read_lock(); 3210 | 3211 | gtp = gtp5g_find_dev(sock_net(skb->sk), info->attrs); 3212 | if (!gtp) { 3213 | GTP5G_ERR(NULL, "Unable to find the gtp device\n"); 3214 | err = -ENODEV; 3215 | goto unlock; 3216 | } 3217 | 3218 | err = gtp5g_gnl_add_qer(gtp, info); 3219 | 3220 | unlock: 3221 | rcu_read_unlock(); 3222 | rtnl_unlock(); 3223 | return err; 3224 | } 3225 | 3226 | static int gtp5g_genl_del_qer(struct sk_buff *skb, struct genl_info *info) 3227 | { 3228 | u32 id; 3229 | struct gtp5g_qer *qer; 3230 | int err = 0; 3231 | 3232 | if (!info->attrs[GTP5G_QER_ID] || 3233 | !info->attrs[GTP5G_LINK]) { 3234 | GTP5G_ERR(NULL, "QER_ID or GTP5g_LINK is not present\n"); 3235 | return -EINVAL; 3236 | } 3237 | 3238 | id = nla_get_u32(info->attrs[GTP5G_QER_ID]); 3239 | 3240 | rcu_read_lock(); 3241 | qer = gtp5g_find_qer(sock_net(skb->sk), info->attrs); 3242 | if (IS_ERR(qer)) { 3243 | err = PTR_ERR(qer); 3244 | GTP5G_ERR(qer->dev, "QER-Del: qer(%u) fail\n", id); 3245 | goto unlock; 3246 | } 3247 | 3248 | GTP5G_LOG(qer->dev, "QER-Del: QER id(%u) success\n", id); 3249 | qer_context_delete(qer); 3250 | unlock: 3251 | rcu_read_unlock(); 3252 | return err; 3253 | } 3254 | 3255 | static int gtp5g_genl_fill_qer(struct sk_buff *skb, u32 snd_portid, u32 snd_seq, 3256 | u32 type, struct gtp5g_qer *qer) 3257 | { 3258 | void *genlh; 3259 | int cnt; 3260 | struct nlattr *nest_mbr_param, *nest_gbr_param; 3261 | struct gtp5g_dev *gtp = netdev_priv(qer->dev); 3262 | struct hlist_head *head; 3263 | struct gtp5g_pdr *pdr; 3264 | 3265 | u16 *u16_buf = kzalloc(0xff * sizeof(u16), GFP_KERNEL); 3266 | if (!u16_buf) { 3267 | GTP5G_ERR(NULL, "Failed to allocated mmeory\n"); 3268 | return -EMSGSIZE; 3269 | } 3270 | 3271 | genlh = genlmsg_put(skb, snd_portid, snd_seq, 3272 | >p5g_genl_family, 0, type); 3273 | if (!genlh) { 3274 | GTP5G_ERR(NULL, "Failed to get genlh snd_port_id(%#x)" 3275 | " \t snd_seq(%#x) type(%#x)\n", 3276 | snd_portid, snd_seq, type); 3277 | goto genlmsg_fail; 3278 | } 3279 | 3280 | /* QER_ID & GATE */ 3281 | if (nla_put_u32(skb, GTP5G_QER_ID, qer->id) || 3282 | nla_put_u8(skb, GTP5G_QER_GATE, qer->ul_dl_gate)) 3283 | goto genlmsg_fail; 3284 | 3285 | /* MBR */ 3286 | if (!(nest_mbr_param = nla_nest_start(skb, GTP5G_QER_MBR))) 3287 | goto genlmsg_fail; 3288 | 3289 | if (nla_put_u32(skb, GTP5G_QER_MBR_UL_HIGH32, qer->mbr.ul_high) || 3290 | nla_put_u8(skb, GTP5G_QER_MBR_UL_LOW8, qer->mbr.ul_low) || 3291 | nla_put_u32(skb, GTP5G_QER_MBR_DL_HIGH32, qer->mbr.dl_high) || 3292 | nla_put_u8(skb, GTP5G_QER_MBR_DL_LOW8, qer->mbr.dl_low)) 3293 | goto genlmsg_fail; 3294 | 3295 | nla_nest_end(skb, nest_mbr_param); 3296 | 3297 | /* GBR */ 3298 | if (!(nest_gbr_param = nla_nest_start(skb, GTP5G_QER_GBR))) 3299 | goto genlmsg_fail; 3300 | 3301 | if (nla_put_u32(skb, GTP5G_QER_GBR_UL_HIGH32, qer->gbr.ul_high) || 3302 | nla_put_u8(skb, GTP5G_QER_GBR_UL_LOW8, qer->gbr.ul_low) || 3303 | nla_put_u32(skb, GTP5G_QER_GBR_DL_HIGH32, qer->gbr.dl_high) || 3304 | nla_put_u8(skb, GTP5G_QER_GBR_DL_LOW8, qer->gbr.dl_low)) 3305 | goto genlmsg_fail; 3306 | 3307 | nla_nest_end(skb, nest_gbr_param); 3308 | 3309 | /* CORR_ID, RQI, QFI, PPI, RCSR */ 3310 | if (nla_put_u32(skb, GTP5G_QER_CORR_ID, qer->qer_corr_id) || 3311 | nla_put_u8(skb, GTP5G_QER_RQI, qer->rqi) || 3312 | nla_put_u8(skb, GTP5G_QER_QFI, qer->qfi) || 3313 | nla_put_u8(skb, GTP5G_QER_PPI, qer->ppi) || 3314 | nla_put_u8(skb, GTP5G_QER_RCSR, qer->rcsr)) 3315 | goto genlmsg_fail; 3316 | 3317 | cnt = 0; 3318 | head = >p->related_qer_hash[u32_hashfn(qer->id) % gtp->hash_size]; 3319 | hlist_for_each_entry_rcu(pdr, head, hlist_related_qer) { 3320 | if (cnt >= 0xff) 3321 | goto genlmsg_fail; 3322 | 3323 | if (*pdr->qer_id == qer->id) 3324 | u16_buf[cnt++] = pdr->id; 3325 | } 3326 | 3327 | if (cnt) { 3328 | if (nla_put(skb, 3329 | GTP5G_QER_RELATED_TO_PDR, 3330 | (cnt * sizeof(u16) / sizeof(char)), 3331 | u16_buf)) 3332 | goto genlmsg_fail; 3333 | } 3334 | 3335 | kfree(u16_buf); 3336 | genlmsg_end(skb, genlh); 3337 | return 0; 3338 | 3339 | genlmsg_fail: 3340 | kfree(u16_buf); 3341 | genlmsg_cancel(skb, genlh); 3342 | return -EMSGSIZE; 3343 | } 3344 | 3345 | static struct gtp5g_qer *gtp5g_find_qer_by_link(struct net *net, struct nlattr *nla[]) 3346 | { 3347 | struct gtp5g_dev *gtp; 3348 | 3349 | gtp = gtp5g_find_dev(net, nla); 3350 | if (!gtp) 3351 | return ERR_PTR(-ENODEV); 3352 | 3353 | if (nla[GTP5G_QER_ID]) { 3354 | u32 id = nla_get_u32(nla[GTP5G_QER_ID]); 3355 | return qer_find_by_id(gtp, id); 3356 | } 3357 | 3358 | return ERR_PTR(-EINVAL); 3359 | } 3360 | 3361 | static struct gtp5g_qer *gtp5g_find_qer(struct net *net, struct nlattr *nla[]) 3362 | { 3363 | struct gtp5g_qer *qer; 3364 | 3365 | if (nla[GTP5G_LINK]) 3366 | qer = gtp5g_find_qer_by_link(net, nla); 3367 | else 3368 | qer = ERR_PTR(-EINVAL); 3369 | 3370 | if (!qer) 3371 | qer = ERR_PTR(-ENOENT); 3372 | 3373 | return qer; 3374 | } 3375 | 3376 | static int gtp5g_genl_get_qer(struct sk_buff *skb, struct genl_info *info) 3377 | { 3378 | struct gtp5g_qer *qer; 3379 | struct sk_buff *skb_ack; 3380 | int err; 3381 | 3382 | if (!info->attrs[GTP5G_QER_ID]) { 3383 | GTP5G_ERR(NULL, "QER ID is not present\n"); 3384 | return -EINVAL; 3385 | } 3386 | 3387 | rcu_read_lock(); 3388 | 3389 | qer = gtp5g_find_qer(sock_net(skb->sk), info->attrs); 3390 | if (IS_ERR(qer)) { 3391 | GTP5G_ERR(NULL, "Failed to find QER\n"); 3392 | err = PTR_ERR(qer); 3393 | goto unlock; 3394 | } 3395 | 3396 | skb_ack = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC); 3397 | if (!skb_ack) { 3398 | err = -ENOMEM; 3399 | goto unlock; 3400 | } 3401 | 3402 | err = gtp5g_genl_fill_qer(skb_ack, 3403 | NETLINK_CB(skb).portid, 3404 | info->snd_seq, 3405 | info->nlhdr->nlmsg_type, 3406 | qer); 3407 | if (err < 0) { 3408 | GTP5G_ERR(NULL, "Failed to fil the qer\n"); 3409 | goto freebuf; 3410 | } 3411 | 3412 | rcu_read_unlock(); 3413 | return genlmsg_unicast(genl_info_net(info), skb_ack, info->snd_portid); 3414 | 3415 | freebuf: 3416 | kfree_skb(skb_ack); 3417 | unlock: 3418 | rcu_read_unlock(); 3419 | return err; 3420 | } 3421 | 3422 | static int gtp5g_genl_dump_qer(struct sk_buff *skb, struct netlink_callback *cb) 3423 | { 3424 | /* netlink_callback->args 3425 | * args[0] : index of gtp5g dev id 3426 | * args[1] : index of gtp5g hash entry id in dev 3427 | * args[2] : index of gtp5g qer id 3428 | * args[5] : set non-zero means it is finished 3429 | */ 3430 | struct gtp5g_dev *gtp, *last_gtp = (struct gtp5g_dev *)cb->args[0]; 3431 | struct net *net = sock_net(skb->sk); 3432 | struct gtp5g_net *gn = net_generic(net, gtp5g_net_id); 3433 | int i, last_hash_entry_id = cb->args[1], ret; 3434 | u32 qer_id = cb->args[2]; 3435 | struct gtp5g_qer *qer; 3436 | 3437 | if (cb->args[5]) { 3438 | GTP5G_ERR(NULL, "Invalid args\n"); 3439 | return 0; 3440 | } 3441 | 3442 | list_for_each_entry_rcu(gtp, &gn->gtp5g_dev_list, list) { 3443 | if (last_gtp && last_gtp != gtp) 3444 | continue; 3445 | else 3446 | last_gtp = NULL; 3447 | 3448 | for (i = last_hash_entry_id; i < gtp->hash_size; i++) { 3449 | hlist_for_each_entry_rcu(qer, >p->qer_id_hash[i], hlist_id) { 3450 | if (qer_id && qer_id != qer->id) 3451 | continue; 3452 | else 3453 | qer_id = 0; 3454 | 3455 | ret = gtp5g_genl_fill_qer(skb, 3456 | NETLINK_CB(cb->skb).portid, 3457 | cb->nlh->nlmsg_seq, 3458 | cb->nlh->nlmsg_type, 3459 | qer); 3460 | if (ret < 0) { 3461 | cb->args[0] = (unsigned long) gtp; 3462 | cb->args[1] = i; 3463 | cb->args[2] = qer->id; 3464 | goto out; 3465 | } 3466 | } 3467 | } 3468 | } 3469 | 3470 | cb->args[5] = 1; 3471 | 3472 | out: 3473 | return skb->len; 3474 | } 3475 | 3476 | static const struct nla_policy gtp5g_genl_pdr_policy[GTP5G_PDR_ATTR_MAX + 1] = { 3477 | [GTP5G_PDR_ID] = { .type = NLA_U32, }, 3478 | [GTP5G_PDR_PRECEDENCE] = { .type = NLA_U32, }, 3479 | [GTP5G_PDR_PDI] = { .type = NLA_NESTED, }, 3480 | [GTP5G_OUTER_HEADER_REMOVAL] = { .type = NLA_U8, }, 3481 | [GTP5G_PDR_FAR_ID] = { .type = NLA_U32, }, 3482 | [GTP5G_PDR_QER_ID] = { .type = NLA_U32, }, 3483 | }; 3484 | 3485 | static const struct nla_policy gtp5g_genl_far_policy[GTP5G_FAR_ATTR_MAX + 1] = { 3486 | [GTP5G_FAR_ID] = { .type = NLA_U32, }, 3487 | [GTP5G_FAR_APPLY_ACTION] = { .type = NLA_U8, }, 3488 | [GTP5G_FAR_FORWARDING_PARAMETER] = { .type = NLA_NESTED, }, 3489 | }; 3490 | 3491 | static const struct nla_policy gtp5g_genl_qer_policy[GTP5G_QER_ATTR_MAX + 1] = { 3492 | [GTP5G_QER_ID] = { .type = NLA_U32, }, 3493 | [GTP5G_QER_GATE] = { .type = NLA_U8, }, 3494 | [GTP5G_QER_MBR] = { .type = NLA_NESTED, }, 3495 | [GTP5G_QER_GBR] = { .type = NLA_NESTED, }, 3496 | [GTP5G_QER_CORR_ID] = { .type = NLA_U32, }, 3497 | [GTP5G_QER_RQI] = { .type = NLA_U8, }, 3498 | [GTP5G_QER_QFI] = { .type = NLA_U8, }, 3499 | [GTP5G_QER_PPI] = { .type = NLA_U8, }, 3500 | [GTP5G_QER_RCSR] = { .type = NLA_U8, }, 3501 | }; 3502 | 3503 | static const struct genl_ops gtp5g_genl_ops[] = { 3504 | { 3505 | .cmd = GTP5G_CMD_ADD_PDR, 3506 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3507 | .doit = gtp5g_genl_add_pdr, 3508 | // .policy = gtp5g_genl_pdr_policy, 3509 | .flags = GENL_ADMIN_PERM, 3510 | }, 3511 | { 3512 | .cmd = GTP5G_CMD_DEL_PDR, 3513 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3514 | .doit = gtp5g_genl_del_pdr, 3515 | // .policy = gtp5g_genl_pdr_policy, 3516 | .flags = GENL_ADMIN_PERM, 3517 | }, 3518 | { 3519 | .cmd = GTP5G_CMD_GET_PDR, 3520 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3521 | .doit = gtp5g_genl_get_pdr, 3522 | .dumpit = gtp5g_genl_dump_pdr, 3523 | // .policy = gtp5g_genl_pdr_policy, 3524 | .flags = GENL_ADMIN_PERM, 3525 | }, 3526 | { 3527 | .cmd = GTP5G_CMD_ADD_FAR, 3528 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3529 | .doit = gtp5g_genl_add_far, 3530 | // .policy = gtp5g_genl_far_policy, 3531 | .flags = GENL_ADMIN_PERM, 3532 | }, 3533 | { 3534 | .cmd = GTP5G_CMD_DEL_FAR, 3535 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3536 | .doit = gtp5g_genl_del_far, 3537 | // .policy = gtp5g_genl_far_policy, 3538 | .flags = GENL_ADMIN_PERM, 3539 | }, 3540 | { 3541 | .cmd = GTP5G_CMD_GET_FAR, 3542 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3543 | .doit = gtp5g_genl_get_far, 3544 | .dumpit = gtp5g_genl_dump_far, 3545 | // .policy = gtp5g_genl_far_policy, 3546 | .flags = GENL_ADMIN_PERM, 3547 | }, 3548 | { 3549 | .cmd = GTP5G_CMD_ADD_QER, 3550 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3551 | .doit = gtp5g_genl_add_qer, 3552 | // .policy = gtp5g_genl_qer_policy, 3553 | .flags = GENL_ADMIN_PERM, 3554 | }, 3555 | { 3556 | .cmd = GTP5G_CMD_DEL_QER, 3557 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3558 | .doit = gtp5g_genl_del_qer, 3559 | // .policy = gtp5g_genl_qer_policy, 3560 | .flags = GENL_ADMIN_PERM, 3561 | }, 3562 | { 3563 | .cmd = GTP5G_CMD_GET_QER, 3564 | // .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 3565 | .doit = gtp5g_genl_get_qer, 3566 | .dumpit = gtp5g_genl_dump_qer, 3567 | // .policy = gtp5g_genl_qer_policy, 3568 | .flags = GENL_ADMIN_PERM, 3569 | }, 3570 | 3571 | }; 3572 | 3573 | static struct genl_family gtp5g_genl_family __ro_after_init = { 3574 | .name = "gtp5g", 3575 | .version = 0, 3576 | .hdrsize = 0, 3577 | .maxattr = GTP5G_ATTR_MAX, 3578 | .netnsok = true, 3579 | .module = THIS_MODULE, 3580 | .ops = gtp5g_genl_ops, 3581 | .n_ops = ARRAY_SIZE(gtp5g_genl_ops), 3582 | }; 3583 | 3584 | static int __net_init gtp5g_net_init(struct net *net) 3585 | { 3586 | struct gtp5g_net *gn = net_generic(net, gtp5g_net_id); 3587 | 3588 | INIT_LIST_HEAD(&gn->gtp5g_dev_list); 3589 | return 0; 3590 | } 3591 | 3592 | static void __net_exit gtp5g_net_exit(struct net *net) 3593 | { 3594 | struct gtp5g_net *gn = net_generic(net, gtp5g_net_id); 3595 | struct gtp5g_dev *gtp; 3596 | LIST_HEAD(list); 3597 | 3598 | rtnl_lock(); 3599 | list_for_each_entry(gtp, &gn->gtp5g_dev_list, list) 3600 | gtp5g_dellink(gtp->dev, &list); 3601 | 3602 | unregister_netdevice_many(&list); 3603 | rtnl_unlock(); 3604 | } 3605 | 3606 | static struct pernet_operations gtp5g_net_ops = { 3607 | .init = gtp5g_net_init, 3608 | .exit = gtp5g_net_exit, 3609 | .id = >p5g_net_id, 3610 | .size = sizeof(struct gtp5g_net), 3611 | }; 3612 | 3613 | struct proc_dir_entry *proc_gtp5g = NULL; 3614 | struct proc_dir_entry *proc_gtp5g_dbg = NULL; 3615 | struct proc_dir_entry *proc_gtp5g_pdr = NULL; 3616 | struct proc_gtp5g_pdr proc_pdr; 3617 | u16 proc_pdr_id = 0; 3618 | 3619 | static int gtp5g_dbg_read(struct seq_file *s, void *v) 3620 | { 3621 | seq_printf(s, "gtp5g kerenl debug level range: 0~4\n"); 3622 | seq_printf(s, "\t 0 -> Logging\n"); 3623 | seq_printf(s, "\t 1 -> Error(default)\n"); 3624 | seq_printf(s, "\t 2 -> Warning\n"); 3625 | seq_printf(s, "\t 3 -> Information\n"); 3626 | seq_printf(s, "\t 4 -> Trace\n"); 3627 | seq_printf(s, "Current: %d\n", dbg_trace_lvl); 3628 | return 0; 3629 | } 3630 | 3631 | static ssize_t proc_dbg_write(struct file *filp, const char __user *buffer, 3632 | size_t len, loff_t *dptr) 3633 | { 3634 | char buf[16]; 3635 | unsigned long buf_len = min(len, sizeof(buf) - 1); 3636 | int dbg; 3637 | 3638 | if (copy_from_user(buf, buffer, buf_len)) { 3639 | GTP5G_ERR(NULL, "Failed to read buffer: %s\n", buffer); 3640 | goto err; 3641 | } 3642 | 3643 | buf[buf_len] = 0; 3644 | if (sscanf(buf, "%d", &dbg) != 1) { 3645 | GTP5G_ERR(NULL, "Failed to read debug level: %s\n", buffer); 3646 | goto err; 3647 | } 3648 | 3649 | if (dbg < 0 || dbg > 4) { 3650 | GTP5G_ERR(NULL, "Failed to set debug level: %d <0 or >4\n", dbg); 3651 | goto err; 3652 | } 3653 | 3654 | dbg_trace_lvl = dbg; 3655 | return strnlen(buf, buf_len); 3656 | err: 3657 | return -1; 3658 | } 3659 | 3660 | static int proc_dbg_read(struct inode *inode, struct file *file) 3661 | { 3662 | return single_open(file, gtp5g_dbg_read, NULL); 3663 | } 3664 | 3665 | static int gtp5g_pdr_read(struct seq_file *s, void *v) 3666 | { 3667 | if (!proc_pdr_id) { 3668 | seq_printf(s, "Given PDR ID does not exists\n"); 3669 | return -1; 3670 | } 3671 | 3672 | seq_printf(s, "PDR: \n"); 3673 | seq_printf(s, "\t ID : %u\n", proc_pdr.id); 3674 | seq_printf(s, "\t Precedence: %u\n", proc_pdr.precedence); 3675 | seq_printf(s, "\t OHR: %u\n", proc_pdr.ohr); 3676 | seq_printf(s, "\t Role Addr4: %#08x\n", ntohl(proc_pdr.role_addr4)); 3677 | seq_printf(s, "\t PDI UE Addr4: %#08x\n", ntohl(proc_pdr.pdi_ue_addr4)); 3678 | seq_printf(s, "\t PDI TEID: %#08x\n", ntohl(proc_pdr.pdi_fteid)); 3679 | seq_printf(s, "\t PDU GTPU Addr4: %#08x\n", ntohl(proc_pdr.pdi_gtpu_addr4)); 3680 | seq_printf(s, "\t FAR ID: %u\n", proc_pdr.far_id); 3681 | seq_printf(s, "\t QER ID: %u\n", proc_pdr.qer_id); 3682 | return 0; 3683 | } 3684 | 3685 | static ssize_t proc_pdr_write(struct file *filp, const char __user *buffer, 3686 | size_t len, loff_t *dptr) 3687 | { 3688 | char buf[128], dev_name[32]; 3689 | u8 found = 0; 3690 | unsigned long buf_len = min(sizeof(buf) - 1, len); 3691 | struct gtp5g_pdr *pdr; 3692 | struct gtp5g_dev *gtp; 3693 | 3694 | if (copy_from_user(buf, buffer, buf_len)) { 3695 | GTP5G_ERR(NULL, "Failed to read buffer: %s\n", buf); 3696 | goto err; 3697 | } 3698 | 3699 | buf[buf_len] = 0; 3700 | if (sscanf(buf, "%s %hu", dev_name, &proc_pdr_id) != 2) { 3701 | GTP5G_ERR(NULL, "proc write of PDR Dev & ID: %s is not valid\n", buf); 3702 | goto err; 3703 | } 3704 | 3705 | list_for_each_entry_rcu(gtp, &proc_gtp5g_dev, proc_list) { 3706 | if (strcmp(dev_name, netdev_name(gtp->dev)) == 0) { 3707 | found = 1; 3708 | break; 3709 | } 3710 | } 3711 | if (!found) { 3712 | GTP5G_ERR(NULL, "Given dev: %s not exists\n", dev_name); 3713 | goto err; 3714 | } 3715 | 3716 | pdr = pdr_find_by_id(gtp, proc_pdr_id); 3717 | if (!pdr) { 3718 | GTP5G_ERR(NULL, "Given PDR ID : %u not exists\n", proc_pdr_id); 3719 | goto err; 3720 | } 3721 | 3722 | memset(&proc_pdr, 0, sizeof(proc_pdr)); 3723 | proc_pdr.id = pdr->id; 3724 | proc_pdr.precedence = pdr->precedence; 3725 | 3726 | if (pdr->outer_header_removal) 3727 | proc_pdr.ohr = *pdr->outer_header_removal; 3728 | 3729 | if (pdr->role_addr_ipv4.s_addr) 3730 | proc_pdr.role_addr4 = pdr->role_addr_ipv4.s_addr; 3731 | 3732 | if (pdr->pdi) { 3733 | if (pdr->pdi->ue_addr_ipv4) 3734 | proc_pdr.pdi_ue_addr4 = pdr->pdi->ue_addr_ipv4->s_addr; 3735 | if (pdr->pdi->f_teid) { 3736 | proc_pdr.pdi_fteid = pdr->pdi->f_teid->teid; 3737 | proc_pdr.pdi_gtpu_addr4 = pdr->pdi->f_teid->gtpu_addr_ipv4.s_addr; 3738 | } 3739 | } 3740 | 3741 | if (pdr->far_id) 3742 | proc_pdr.far_id = *pdr->far_id; 3743 | 3744 | if (pdr->qer_id) 3745 | proc_pdr.qer_id = *pdr->qer_id; 3746 | 3747 | return strnlen(buf, buf_len); 3748 | err: 3749 | proc_pdr_id = 0; 3750 | return -1; 3751 | } 3752 | 3753 | static int proc_pdr_read(struct inode *inode, struct file *file) 3754 | { 3755 | return single_open(file, gtp5g_pdr_read, NULL); 3756 | } 3757 | 3758 | #if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 6, 0) 3759 | static const struct proc_ops proc_gtp5g_dbg_ops = { 3760 | .proc_open = proc_dbg_read, 3761 | .proc_read = seq_read, 3762 | .proc_write = proc_dbg_write, 3763 | .proc_lseek = seq_lseek, 3764 | .proc_release = single_release, 3765 | }; 3766 | #else 3767 | static const struct file_operations proc_gtp5g_dbg_ops = { 3768 | .owner = THIS_MODULE, 3769 | .open = proc_dbg_read, 3770 | .read = seq_read, 3771 | .write = proc_dbg_write, 3772 | .llseek = seq_lseek, 3773 | .release = single_release, 3774 | }; 3775 | #endif 3776 | 3777 | #if LINUX_VERSION_CODE >= KERNEL_VERSION(5, 6, 0) 3778 | static const struct proc_ops proc_gtp5g_pdr_ops = { 3779 | .proc_open = proc_pdr_read, 3780 | .proc_read = seq_read, 3781 | .proc_write = proc_pdr_write, 3782 | .proc_lseek = seq_lseek, 3783 | .proc_release = single_release, 3784 | }; 3785 | #else 3786 | static const struct file_operations proc_gtp5g_pdr_ops = { 3787 | .owner = THIS_MODULE, 3788 | .open = proc_pdr_read, 3789 | .read = seq_read, 3790 | .write = proc_pdr_write, 3791 | .llseek = seq_lseek, 3792 | .release = single_release, 3793 | }; 3794 | #endif 3795 | 3796 | static int __init gtp5g_init(void) 3797 | { 3798 | int err; 3799 | 3800 | GTP5G_LOG(NULL, "Gtp5g Module initialization Ver: %s\n", DRV_VERSION); 3801 | 3802 | INIT_LIST_HEAD(&proc_gtp5g_dev); 3803 | 3804 | get_random_bytes(>p5g_h_initval, sizeof(gtp5g_h_initval)); 3805 | 3806 | err = rtnl_link_register(>p5g_link_ops); 3807 | if (err < 0) { 3808 | GTP5G_ERR(NULL, "Failed to register rtnl\n"); 3809 | goto error_out; 3810 | } 3811 | 3812 | err = genl_register_family(>p5g_genl_family); 3813 | if (err < 0) { 3814 | GTP5G_ERR(NULL, "Failed to register generic\n"); 3815 | goto unreg_rtnl_link; 3816 | } 3817 | 3818 | err = register_pernet_subsys(>p5g_net_ops); 3819 | if (err < 0) { 3820 | GTP5G_ERR(NULL, "Failed to register namespace\n"); 3821 | goto unreg_genl_family; 3822 | } 3823 | 3824 | proc_gtp5g = proc_mkdir("gtp5g", NULL); 3825 | if (!proc_gtp5g) { 3826 | GTP5G_ERR(NULL, "Failed to create /proc/gtp5g\n"); 3827 | goto unreg_pernet; 3828 | } 3829 | 3830 | proc_gtp5g_dbg = proc_create("dbg", (S_IFREG | S_IRUGO | S_IWUGO), proc_gtp5g, &proc_gtp5g_dbg_ops); 3831 | if (!proc_gtp5g_dbg) { 3832 | GTP5G_ERR(NULL, "Failed to create /proc/gtp5g/dbg\n"); 3833 | goto remove_gtp5g_proc; 3834 | } 3835 | 3836 | proc_gtp5g_pdr = proc_create("pdr", (S_IFREG | S_IRUGO | S_IWUGO), proc_gtp5g, &proc_gtp5g_pdr_ops); 3837 | if (!proc_gtp5g_pdr) { 3838 | GTP5G_ERR(NULL, "Failed to create /proc/gtp5g/pdr\n"); 3839 | goto remove_dbg_proc; 3840 | } 3841 | 3842 | GTP5G_LOG(NULL, "5G GTP module loaded (pdr ctx size %zd bytes)\n", 3843 | sizeof(struct gtp5g_pdr)); 3844 | 3845 | return 0; 3846 | 3847 | remove_dbg_proc: 3848 | remove_proc_entry("dbg", proc_gtp5g); 3849 | remove_gtp5g_proc: 3850 | remove_proc_entry("gtp5g", NULL); 3851 | unreg_pernet: 3852 | unregister_pernet_subsys(>p5g_net_ops); 3853 | unreg_genl_family: 3854 | genl_unregister_family(>p5g_genl_family); 3855 | unreg_rtnl_link: 3856 | rtnl_link_unregister(>p5g_link_ops); 3857 | error_out: 3858 | return err; 3859 | } 3860 | late_initcall(gtp5g_init); 3861 | 3862 | static void __exit gtp5g_fini(void) 3863 | { 3864 | genl_unregister_family(>p5g_genl_family); 3865 | rtnl_link_unregister(>p5g_link_ops); 3866 | unregister_pernet_subsys(>p5g_net_ops); 3867 | 3868 | remove_proc_entry("pdr", proc_gtp5g); 3869 | remove_proc_entry("dbg", proc_gtp5g); 3870 | remove_proc_entry("gtp5g", NULL); 3871 | 3872 | GTP5G_LOG(NULL, "5G GTP module unloaded\n"); 3873 | } 3874 | module_exit(gtp5g_fini); 3875 | 3876 | MODULE_LICENSE("GPL"); 3877 | MODULE_AUTHOR("Yao-Wen Chang "); 3878 | MODULE_AUTHOR("Muthuraman "); 3879 | MODULE_DESCRIPTION("Interface for 5G GTP encapsulated traffic"); 3880 | MODULE_VERSION(DRV_VERSION); 3881 | MODULE_ALIAS_RTNL_LINK("gtp5g"); 3882 | MODULE_ALIAS_GENL_FAMILY("gtp5g"); 3883 | -------------------------------------------------------------------------------- /gtp5g.h: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: GPL-2.0 */ 2 | #ifndef _GTP_H_ 3 | #define _GTP_H_ 4 | 5 | /** ----------------------------------------------------------- 6 | * BEGIN OF General GTP protocol related definitions 7 | * ----------------------------------------------------------- 8 | */ 9 | 10 | #define GTP1U_PORT 2152 11 | 12 | #define GTP_TPDU 255 13 | #define GTP_EMARK 254 14 | 15 | /* According to 3GPP TS 29.060. */ 16 | struct gtpv1_hdr { 17 | __u8 flags; 18 | #define GTPV1_HDR_FLG_NPDU 0x01 19 | #define GTPV1_HDR_FLG_SEQ 0x02 20 | #define GTPV1_HDR_FLG_EXTHDR 0x04 21 | #define GTPV1_HDR_FLG_MASK 0x07 22 | 23 | __u8 type; 24 | __be16 length; 25 | __be32 tid; 26 | } __attribute__((packed)) gtpv1_hdr_t; 27 | 28 | typedef struct gtp1_hdr_opt { 29 | __be16 seq_number; 30 | __u8 NPDU; 31 | __u8 next_ehdr_type; 32 | /** 3GPP TS 29.281 33 | * From Figure 5.2.1-2 Definition of Extension Header Type 34 | */ 35 | #define GTPV1_NEXT_EXT_HDR_TYPE_00 0x00 /* No More extension */ 36 | #define GTPV1_NEXT_EXT_HDR_TYPE_03 0x03 /* Long PDCP PDU Number */ 37 | #define GTPV1_NEXT_EXT_HDR_TYPE_20 0x20 /* Service Class Indicator */ 38 | #define GTPV1_NEXT_EXT_HDR_TYPE_40 0x40 /* UDP Port */ 39 | #define GTPV1_NEXT_EXT_HDR_TYPE_81 0x81 /* RAN Container */ 40 | #define GTPV1_NEXT_EXT_HDR_TYPE_82 0x82 /* Long PDCP PDU Number */ 41 | #define GTPV1_NEXT_EXT_HDR_TYPE_83 0x83 /* Xw RAN Container */ 42 | #define GTPV1_NEXT_EXT_HDR_TYPE_84 0x84 /* NR RAN Container */ 43 | #define GTPV1_NEXT_EXT_HDR_TYPE_85 0x85 /* PDU Session Container */ 44 | #define GTPV1_NEXT_EXT_HDR_TYPE_C0 0xc0 /* PDCP PDU Number */ 45 | 46 | } __attribute__((packed)) gtpv1_hdr_opt_t; 47 | 48 | 49 | /** 3GPP TS 29.281 50 | * Section 5.2.1 General format of the GTP-U Extension Header 51 | * 52 | * +-----------------------------+ 53 | * Octets 1 | Extension Header Length | 54 | * +-----------------------------+ 55 | * 2 – m | Extension Header Content | 56 | * +-----------------------------+ 57 | * m+1 | Next Extension Header Type | 58 | * +-----------------------------+ 59 | * 60 | * Note: If no more header type then the value of Next Extension Header Type 61 | * is ZERO. 62 | * 63 | * 64 | * Section 5.2.2 Extension Header types 65 | */ 66 | 67 | /** TS 29.281 68 | * 5.2.2.1 UDP Port 69 | * 70 | * */ 71 | struct gtp1_hdr_ext_udp_port { 72 | __u8 length; 73 | __be16 udp_port_num; 74 | __u8 next_ehdr_type; 75 | } __attribute__((packed)) ext_dup_port_t; 76 | 77 | /** TS 29.281 78 | * 5.2.2.2 PDCP PDU Number 79 | * 80 | * This value shall be used by a source eNB or gNB 81 | * */ 82 | struct gtp1_hdr_ext_pdcp_pdu_num { 83 | __u8 length; 84 | __u8 pdcp_pdu_num0; 85 | __u8 pdcp_pdu_num1; 86 | __u8 next_ehdr_type; 87 | } __attribute__((packed)) ext_pdcp_pdu_num_t; 88 | 89 | /** TS 29.281 90 | * 5.2.2.2A Long PDU Session container 91 | * 92 | */ 93 | struct gtp1_hdr_ext_lpdcp_pdu_num { 94 | __u8 length; 95 | __u8 spare0_pdcp_pdu_num0; 96 | __u8 pdcp_pdu_num1; 97 | __u8 pdcp_pdu_num2; 98 | __u8 spare1; 99 | __u8 spare2; 100 | __u8 spare3; 101 | __u8 next_ehdr_type; 102 | } __attribute__((packed)) ext_lpdcp_pdu_num_t; 103 | 104 | /** TS 29.281 105 | * 5.2.2.3 Service Class Indicator 106 | * 107 | */ 108 | struct gtp1_hdr_ext_service_cls { 109 | __u8 length; 110 | __u8 service_cls_indicator; 111 | __u8 spare; 112 | __u8 next_ehdr_type; 113 | } __attribute__((packed)) ext_service_cls_t; 114 | 115 | /** TS 29.281 116 | * 5.2.2.4 RAN container 117 | * 118 | */ 119 | struct gtp1_hdr_ext_ran_container { 120 | __u8 length; 121 | __u8 ran_ctr[0]; 122 | __u8 next_ehdr_type; 123 | } __attribute__((packed)) ext_ran_ctr_t; 124 | 125 | /** TS 29.281 126 | * 5.2.2.5 Xw RAN container 127 | * 128 | */ 129 | struct gtp1_hdr_ext_xw_ran_ctr { 130 | __u8 length; 131 | __u8 xw_ran_ctr[0]; 132 | __u8 next_ehdr_type; 133 | } __attribute__((packed)) ext_xw_ran_ctr_t; 134 | 135 | 136 | /** TS 29.281 137 | * 5.2.2.7 NR RAN container 138 | * 139 | */ 140 | typedef struct gtp1_hdr_ext_nr_ran_ctr { 141 | __u8 length; 142 | __u8 nr_ran_ctr[0]; 143 | __u8 next_ehdr_type; 144 | } __attribute__((packed)) ext_nr_ran_ctr_t; 145 | 146 | /** TS 29.281 147 | * 5.2.2.7 PDU Session container 148 | * > Extension Type 133(0x85) 149 | * > Transmitted in a G-PDU over N3 and N9 150 | * 151 | * @pdu_sess_container 152 | * - variable length field 153 | * - Specified in TS 38.415 (Refer. v15.2.0 (2018-12)) in Section 5.5 154 | * -> DL PDU Session Information (PDU Type 0) 155 | * <- TODO: if ppp is set to 1 then ppi SHOULD set 156 | * -> UL PDU Session Information (PDU Type 1) 157 | * 158 | * Note: For a G-PDU with serveral Extension Headers, the PDU Session 159 | * container SHOULD BE the FIRST Extension Header 160 | */ 161 | typedef struct ul_pdu_sess_info { 162 | __u8 spare_qfi; /* Spare(2b) + qfi(6b)*/ 163 | } __attribute__((packed)) ul_pdu_sess_info_t; 164 | 165 | typedef struct dl_pdu_sess_info { 166 | __u8 ppp_rqi_qfi; /* ppp(1b) + rqi(1b) + qfi(6) */ 167 | } __attribute__((packed)) dl_pdu_sess_info_t; 168 | 169 | typedef struct dl_pdu_sess_info_ppi { 170 | __u8 ppi_spare; /* ppi(3b) + spare(5b) */ 171 | __u8 padding[3]; 172 | } __attribute__((packed)) dl_pdu_sess_info_ppi_t; 173 | 174 | typedef struct pdu_sess_ctr { 175 | __u8 type_spare; /* type(4b) + spare(4b) */ 176 | #define PDU_SESSION_INFO_TYPE0 0x00 177 | #define PDU_SESSION_INFO_TYPE1 0x10 178 | union { 179 | ul_pdu_sess_info_t ul; 180 | dl_pdu_sess_info_t dl; 181 | } u; 182 | //dl_pdu_sess_info_ppi_t dl_ppi[0]; 183 | } __attribute__((packed)) pdu_sess_ctr_t; 184 | 185 | typedef struct gtp1_hdr_ext_pdu_sess_ctr { 186 | __u8 length; 187 | pdu_sess_ctr_t pdu_sess_ctr; 188 | __u8 next_ehdr_type; 189 | } __attribute__((packed)) ext_pdu_sess_ctr_t; 190 | 191 | 192 | /** ----------------------------------------------------------- 193 | * END OF General GTP protocol related definitions 194 | * ----------------------------------------------------------- 195 | */ 196 | 197 | /* Maybe add this part to if_link.h */ 198 | enum ifla_gtp5g_role { 199 | GTP5G_ROLE_UPF = 0, 200 | GTP5G_ROLE_RAN, 201 | }; 202 | 203 | enum { 204 | IFLA_GTP5G_UNSPEC, 205 | 206 | IFLA_GTP5G_FD1, 207 | IFLA_GTP5G_PDR_HASHSIZE, 208 | IFLA_GTP5G_ROLE, 209 | 210 | __IFLA_GTP5G_MAX, 211 | }; 212 | #define IFLA_GTP5G_MAX (__IFLA_GTP5G_MAX - 1) 213 | /* end of part */ 214 | 215 | enum gtp5g_cmd { 216 | GTP5G_CMD_UNSPEC = 0, 217 | 218 | GTP5G_CMD_ADD_PDR, 219 | GTP5G_CMD_ADD_FAR, 220 | GTP5G_CMD_ADD_QER, 221 | 222 | GTP5G_CMD_DEL_PDR, 223 | GTP5G_CMD_DEL_FAR, 224 | GTP5G_CMD_DEL_QER, 225 | 226 | GTP5G_CMD_GET_PDR, 227 | GTP5G_CMD_GET_FAR, 228 | GTP5G_CMD_GET_QER, 229 | 230 | __GTP5G_CMD_MAX, 231 | }; 232 | #define GTP5G_CMD_MAX (__GTP5G_CMD_MAX - 1) 233 | 234 | /* This const value need to bigger than the Layer 1 attr size, 235 | * like GTP5G_PDR_ATTR_MAX and GTP5G_FAR_ATTR_MAX 236 | */ 237 | #define GTP5G_ATTR_MAX 0x10 238 | 239 | enum gtp5g_device_attrs { 240 | GTP5G_LINK = 1, 241 | GTP5G_NET_NS_FD, 242 | }; 243 | 244 | enum gtp5g_pdr_attrs { 245 | /* gtp5g_device_attrs in this part */ 246 | 247 | GTP5G_PDR_ID = 3, 248 | GTP5G_PDR_PRECEDENCE, 249 | GTP5G_PDR_PDI, 250 | GTP5G_OUTER_HEADER_REMOVAL, 251 | GTP5G_PDR_FAR_ID, 252 | 253 | /* Not in 3GPP spec, just used for routing */ 254 | GTP5G_PDR_ROLE_ADDR_IPV4, 255 | 256 | /* Not in 3GPP spec, just used for buffering */ 257 | GTP5G_PDR_UNIX_SOCKET_PATH, 258 | 259 | GTP5G_PDR_QER_ID, 260 | 261 | /* Add newly supported feature ON ABOVE 262 | * for compatability with older version of 263 | * free5GC's UPF or libgtp5gnl 264 | * */ 265 | 266 | __GTP5G_PDR_ATTR_MAX, 267 | }; 268 | #define GTP5G_PDR_ATTR_MAX (__GTP5G_PDR_ATTR_MAX - 1) 269 | 270 | /* Nest in GTP5G_PDR_PDI */ 271 | enum gtp5g_pdi_attrs { 272 | GTP5G_PDI_UE_ADDR_IPV4 = 1, 273 | GTP5G_PDI_F_TEID, 274 | GTP5G_PDI_SDF_FILTER, 275 | 276 | __GTP5G_PDI_ATTR_MAX, 277 | }; 278 | #define GTP5G_PDI_ATTR_MAX (__GTP5G_PDI_ATTR_MAX - 1) 279 | 280 | /* Nest in GTP5G_PDI_F_TEID */ 281 | enum gtp5g_f_teid_attrs { 282 | GTP5G_F_TEID_I_TEID = 1, 283 | GTP5G_F_TEID_GTPU_ADDR_IPV4, 284 | 285 | __GTP5G_F_TEID_ATTR_MAX, 286 | }; 287 | #define GTP5G_F_TEID_ATTR_MAX (__GTP5G_F_TEID_ATTR_MAX - 1) 288 | 289 | /* Nest in GTP5G_PDI_SDF_FILTER */ 290 | enum gtp5g_sdf_filter_attrs { 291 | GTP5G_SDF_FILTER_FLOW_DESCRIPTION = 1, 292 | GTP5G_SDF_FILTER_TOS_TRAFFIC_CLASS, 293 | GTP5G_SDF_FILTER_SECURITY_PARAMETER_INDEX, 294 | GTP5G_SDF_FILTER_FLOW_LABEL, 295 | GTP5G_SDF_FILTER_SDF_FILTER_ID, 296 | 297 | __GTP5G_SDF_FILTER_ATTR_MAX, 298 | }; 299 | #define GTP5G_SDF_FILTER_ATTR_MAX (__GTP5G_SDF_FILTER_ATTR_MAX - 1) 300 | 301 | /* Nest in GTP5G_SDF_FILTER_FLOW_DESCRIPTION */ 302 | enum gtp5g_flow_description_attrs { 303 | GTP5G_FLOW_DESCRIPTION_ACTION = 1, // Only "permit" 304 | GTP5G_FLOW_DESCRIPTION_DIRECTION, 305 | GTP5G_FLOW_DESCRIPTION_PROTOCOL, 306 | GTP5G_FLOW_DESCRIPTION_SRC_IPV4, 307 | GTP5G_FLOW_DESCRIPTION_SRC_MASK, 308 | GTP5G_FLOW_DESCRIPTION_DEST_IPV4, 309 | GTP5G_FLOW_DESCRIPTION_DEST_MASK, 310 | GTP5G_FLOW_DESCRIPTION_SRC_PORT, 311 | GTP5G_FLOW_DESCRIPTION_DEST_PORT, 312 | 313 | __GTP5G_FLOW_DESCRIPTION_ATTR_MAX, 314 | }; 315 | #define GTP5G_FLOW_DESCRIPTION_ATTR_MAX (__GTP5G_FLOW_DESCRIPTION_ATTR_MAX - 1) 316 | 317 | enum gtp5g_far_attrs { 318 | /* gtp5g_device_attrs in this part */ 319 | 320 | GTP5G_FAR_ID = 3, 321 | GTP5G_FAR_APPLY_ACTION, 322 | GTP5G_FAR_FORWARDING_PARAMETER, 323 | 324 | /* Not IEs in 3GPP Spec, for other purpose */ 325 | GTP5G_FAR_RELATED_TO_PDR, 326 | 327 | __GTP5G_FAR_ATTR_MAX, 328 | }; 329 | #define GTP5G_FAR_ATTR_MAX (__GTP5G_FAR_ATTR_MAX - 1) 330 | 331 | #define FAR_ACTION_UPSPEC 0x00 332 | #define FAR_ACTION_DROP 0x01 333 | #define FAR_ACTION_FORW 0x02 334 | #define FAR_ACTION_BUFF 0x04 335 | #define FAR_ACTION_MASK 0x07 336 | #define FAR_ACTION_NOCP 0x08 337 | #define FAR_ACTION_DUPL 0x10 338 | 339 | /* Nest in GTP5G_FAR_FORWARDING_PARAMETER */ 340 | enum gtp5g_forwarding_parameter_attrs { 341 | GTP5G_FORWARDING_PARAMETER_OUTER_HEADER_CREATION = 1, 342 | GTP5G_FORWARDING_PARAMETER_FORWARDING_POLICY, 343 | 344 | __GTP5G_FORWARDING_PARAMETER_ATTR_MAX, 345 | }; 346 | #define GTP5G_FORWARDING_PARAMETER_ATTR_MAX (__GTP5G_FORWARDING_PARAMETER_ATTR_MAX - 1) 347 | 348 | /* Nest in GTP5G_FORWARDING_PARAMETER_OUTER_HEADER_CREATION */ 349 | enum gtp5g_outer_header_creation_attrs { 350 | GTP5G_OUTER_HEADER_CREATION_DESCRIPTION = 1, 351 | GTP5G_OUTER_HEADER_CREATION_O_TEID, 352 | GTP5G_OUTER_HEADER_CREATION_PEER_ADDR_IPV4, 353 | GTP5G_OUTER_HEADER_CREATION_PORT, 354 | 355 | __GTP5G_OUTER_HEADER_CREATION_ATTR_MAX, 356 | }; 357 | #define GTP5G_OUTER_HEADER_CREATION_ATTR_MAX (__GTP5G_OUTER_HEADER_CREATION_ATTR_MAX - 1) 358 | 359 | enum { 360 | GTP5G_SDF_FILTER_ACTION_UNSPEC = 0, 361 | 362 | GTP5G_SDF_FILTER_PERMIT, 363 | 364 | __GTP5G_SDF_FILTER_ACTION_MAX, 365 | }; 366 | #define GTP5G_SDF_FILTER_ACTION_MAX (__GTP5G_SDF_FILTER_ACTION_MAX - 1) 367 | 368 | enum { 369 | GTP5G_SDF_FILTER_DIRECTION_UNSPEC = 0, 370 | 371 | GTP5G_SDF_FILTER_IN, 372 | GTP5G_SDF_FILTER_OUT, 373 | 374 | __GTP5G_SDF_FILTER_DIRECTION_MAX, 375 | }; 376 | #define GTP5G_SDF_FILTER_DIRECTION_MAX (__GTP5G_SDF_FILTER_DIRECTION_MAX - 1) 377 | 378 | 379 | /* ------------------------------------------------------------------ 380 | * QER 381 | * ------------------------------------------------------------------ 382 | * */ 383 | enum gtp5g_qer_attrs { 384 | /* gtp5g_device_attrs in this part */ 385 | 386 | GTP5G_QER_ID = 3, 387 | GTP5G_QER_GATE, 388 | GTP5G_QER_MBR, 389 | GTP5G_QER_GBR, 390 | GTP5G_QER_CORR_ID, 391 | GTP5G_QER_RQI, 392 | GTP5G_QER_QFI, 393 | GTP5G_QER_PPI, 394 | GTP5G_QER_RCSR, 395 | 396 | 397 | /* Not IEs in 3GPP Spec, for other purpose */ 398 | GTP5G_QER_RELATED_TO_PDR, 399 | 400 | __GTP5G_QER_ATTR_MAX, 401 | }; 402 | #define GTP5G_QER_ATTR_MAX (__GTP5G_QER_ATTR_MAX - 1) 403 | 404 | /* Nest in GTP5G_QER_MBR */ 405 | enum gtp5g_mbr_attrs { 406 | GTP5G_QER_MBR_UL_HIGH32 = 1, 407 | GTP5G_QER_MBR_UL_LOW8, 408 | GTP5G_QER_MBR_DL_HIGH32, 409 | GTP5G_QER_MBR_DL_LOW8, 410 | 411 | __GTP5G_QER_MBR_ATTR_MAX, 412 | }; 413 | #define GTP5G_QER_MBR_ATTR_MAX (__GTP5G_QER_MBR_ATTR_MAX - 1) 414 | 415 | /* Nest in GTP5G_QER_QBR */ 416 | enum gtp5g_qer_gbr_attrs { 417 | GTP5G_QER_GBR_UL_HIGH32 = 1, 418 | GTP5G_QER_GBR_UL_LOW8, 419 | GTP5G_QER_GBR_DL_HIGH32, 420 | GTP5G_QER_GBR_DL_LOW8, 421 | 422 | __GTP5G_QER_GBR_ATTR_MAX, 423 | }; 424 | #define GTP5G_QER_GBR_ATTR_MAX (__GTP5G_QER_GBR_ATTR_MAX - 1) 425 | 426 | #endif 427 | --------------------------------------------------------------------------------