embox

1
/**
2
 * @file
3
 * @brief Implements udp socket function.
4
 *
5
 * @date 04.02.10
6
 * @author Anton Bondarev
7
 * @author Ilia Vaprol
8
 */
9

10
#include <assert.h>
11
#include <errno.h>
12
#include <stddef.h>
13
#include <string.h>
14
#include <util/math.h>
15
#include <sys/socket.h>
16
#include <netinet/in.h>
17

18
#include "net_sock.h"
19
#include <embox/net/pack.h>
20

21
#include <net/l3/ipv4/ip.h>
22
#include <net/l4/udp.h>
23
#include <net/lib/udp.h>
24
#include <net/sock.h>
25
#include <net/socket/inet_sock.h>
26

27
#include <lib/libds/dlist.h>
28

29
#include <stdlib.h>
30

31
static const struct sock_proto_ops udp_sock_ops_struct;
32
const struct sock_proto_ops *const udp_sock_ops = &udp_sock_ops_struct;
33

34
EMBOX_NET_SOCK(AF_INET, SOCK_DGRAM, IPPROTO_UDP, 1,
35
		udp_sock_ops_struct);
36

37
static int iov_msg_len(struct iovec *iov, int iovlen) {
38
	int len = 0;
39

40
	for (int i = 0; i < iovlen; i++) {
41
		len += iov[i].iov_len;
42
	}
43

44
	return len;
45
}
46

47
int iov_stream_make_queue(size_t len, size_t hdr_size, struct sk_buff_head *queue,
48
		struct sock *sk, const struct sockaddr *sockaddr) {
49
	int to_alloc = len;
50

51
	skb_queue_init(queue);
52

53
	while (to_alloc > 0) {
54
		struct sk_buff *skb = NULL;
55
		size_t actual_len = to_alloc + hdr_size;
56

57
		int ret = sk->o_ops->make_pack(sk, sockaddr, &actual_len, &skb);
58
		if (ret < 0) {
59
			skb_queue_purge(queue);
60
			return ret;
61
		}
62
		skb_queue_push(queue, skb);
63
		to_alloc -= actual_len - hdr_size;
64
	}
65

66
	return len;
67
}
68

69
int iov_dgram_make_queue(size_t len, size_t hdr_size, struct sk_buff_head *queue,
70
		struct sock *sk, const struct sockaddr *sockaddr) {
71

72
	skb_queue_init(queue);
73

74
	struct sk_buff *skb = NULL;
75
	size_t actual_len = len + hdr_size;
76

77
	int ret = sk->o_ops->make_pack(sk, sockaddr, &actual_len, &skb);
78
	if (ret < 0) {
79
		return ret;
80
	}
81

82
	skb_queue_push(queue, skb);
83
	return actual_len - hdr_size;
84
}
85

86
static int skb_queue_iov(struct sk_buff_head *queue, const struct iovec *iov, int iovlen, size_t header_len) {
87
	struct sk_buff *skb = skb_queue_front(queue);
88
	int i_iov = 0;
89
	int skb_pos = 0, iov_pos = 0;
90
	int ret = 0;
91

92
	while (!skb_queue_end(skb, queue) && (i_iov < iovlen)) {
93
		const int to_copy = min(skb->len - skb_pos, iov[i_iov].iov_len - iov_pos);
94
		memcpy(skb->mac.raw + header_len + skb_pos, iov[i_iov].iov_base + iov_pos, to_copy);
95

96
		iov_pos += to_copy;
97
		skb_pos += to_copy;
98
		ret += to_copy;
99

100
		if (skb->len == skb_pos) {
101
			skb = skb_queue_next(skb);
102
			skb_pos = 0;
103
		}
104

105
		if (iov[i_iov].iov_len == iov_pos) {
106
			++i_iov;
107
			iov_pos = 0;
108
		}
109
	}
110

111
	return ret;
112
}
113

114
static int udp_get_udp_offset(struct sk_buff *skb) {
115
	return (unsigned char *) (skb->h.uh) - skb->mac.raw;
116
}
117

118
static int udp_sendmsg(struct sock *sk, struct msghdr *msg, int flags) {
119
	const in_port_t sk_src = sock_inet_get_src_port(sk);
120
	const struct sockaddr_in *const addr_to = (const struct sockaddr_in *)
121
			(msg->msg_name ? msg->msg_name : &to_inet_sock(sk)->dst_in);
122
	struct sk_buff_head queue;
123
	int err;
124

125
	const size_t iov_data_len = iov_msg_len(msg->msg_iov, msg->msg_iovlen);
126

127
	// msg->msg_name could be NULL, sockaddr is OK to be NULL
128
	const struct sockaddr *sockaddr = (const struct sockaddr *)msg->msg_name;
129

130
	const int out_data_len = iov_dgram_make_queue(iov_data_len, UDP_HEADER_SIZE, &queue, sk, sockaddr);
131
	if (out_data_len < 0) {
132
		return out_data_len;
133
	}
134

135
	// FIXME there should be a better way to get offset
136
	const int skb_udp_offset = udp_get_udp_offset(queue.next);
137

138
	const int bytes_copied = skb_queue_iov(&queue, msg->msg_iov, msg->msg_iovlen, skb_udp_offset + UDP_HEADER_SIZE);
139
	if (bytes_copied != out_data_len) {
140
		return -1;
141
	}
142

143
	err = 0;
144
	for (struct sk_buff *skb = skb_queue_pop(&queue); skb; skb = skb_queue_pop(&queue)) {
145
		udp_build(skb->h.uh, sk_src, addr_to->sin_port, skb->len - skb_udp_offset);
146
		udp4_set_check_field(skb->h.uh, skb->nh.iph);
147
		err = sk->o_ops->snd_pack(skb);
148
		if (err < 0) {
149
			break;
150
		}
151
	}
152

153
	if (err < 0) {
154
		skb_queue_purge(&queue);
155
		return err;
156
	}
157

158
	assert(skb_queue_front(&queue) == NULL); // should be empty at this point
159

160
	return out_data_len;
161
}
162

163
static DLIST_DEFINE(udp_sock_list);
164

165
static int udp_fillmsg(struct sock *sk, struct msghdr *msg,
166
		struct sk_buff *skb) {
167
	struct sockaddr_in *inaddr;
168

169
	inaddr = (struct sockaddr_in *)msg->msg_name;
170

171
	inaddr->sin_family = AF_INET;
172
	memcpy(&inaddr->sin_addr, &ip_hdr(skb)->saddr, sizeof(in_addr_t));
173
	inaddr->sin_port = udp_hdr(skb)->source;
174

175
	return 0;
176
}
177

178
static const struct sock_proto_ops udp_sock_ops_struct = {
179
	.sendmsg   = udp_sendmsg,
180
	.recvmsg   = sock_dgram_recvmsg,
181
	.fillmsg   = udp_fillmsg,
182
	.sock_list = &udp_sock_list
183
};
184