source: mainline/uspace/srv/ethip/pdu.c@ 962f03b

lfn serial ticket/834-toolchain-update topic/msim-upgrade topic/simplify-dev-export
Last change on this file since 962f03b was 87e5658c, checked in by Jiri Svoboda <jiri@…>, 14 years ago

Prototype ARP responder.
  • Property mode set to 100644
File size: 6.6 KB
Line 
1/*
2 * Copyright (c) 2012 Jiri Svoboda
3 * All rights reserved.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions
7 * are met:
8 *
9 * - Redistributions of source code must retain the above copyright
10 * notice, this list of conditions and the following disclaimer.
11 * - Redistributions in binary form must reproduce the above copyright
12 * notice, this list of conditions and the following disclaimer in the
13 * documentation and/or other materials provided with the distribution.
14 * - The name of the author may not be used to endorse or promote products
15 * derived from this software without specific prior written permission.
16 *
17 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
18 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
19 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
20 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
21 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
22 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
26 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 */
28
29/** @addtogroup inet
30 * @{
31 */
32/**
33 * @file
34 * @brief
35 */
36
37#include <byteorder.h>
38#include <errno.h>
39#include <io/log.h>
40#include <macros.h>
41#include <mem.h>
42#include <stdlib.h>
43
44#include "ethip.h"
45#include "std.h"
46#include "pdu.h"
47
48static void mac48_encode(mac48_addr_t *addr, void *buf);
49static void mac48_decode(void *data, mac48_addr_t *addr);
50
51#define MAC48_BYTES 6
52
53/** Encode Ethernet PDU. */
54int eth_pdu_encode(eth_frame_t *frame, void **rdata, size_t *rsize)
55{
56 void *data;
57 size_t size;
58 eth_header_t *hdr;
59
60 size = max(sizeof(eth_header_t) + frame->size, ETH_FRAME_MIN_SIZE);
61
62 data = calloc(size, 1);
63 if (data == NULL)
64 return ENOMEM;
65
66 hdr = (eth_header_t *)data;
67 mac48_encode(&frame->src, hdr->src);
68 mac48_encode(&frame->dest, hdr->dest);
69 hdr->etype_len = host2uint16_t_be(frame->etype_len);
70
71 memcpy((uint8_t *)data + sizeof(eth_header_t), frame->data,
72 frame->size);
73
74 *rdata = data;
75 *rsize = size;
76 return EOK;
77}
78
79#include <stdio.h>
80/** Decode Ethernet PDU. */
81int eth_pdu_decode(void *data, size_t size, eth_frame_t *frame)
82{
83 eth_header_t *hdr;
84
85 log_msg(LVL_DEBUG, "eth_pdu_decode()");
86
87 if (size < sizeof(eth_header_t)) {
88 log_msg(LVL_DEBUG, "PDU too short (%zu)", size);
89 return EINVAL;
90 }
91
92 hdr = (eth_header_t *)data;
93
94 frame->size = size - sizeof(eth_header_t);
95 frame->data = calloc(frame->size, 1);
96 if (frame->data == NULL)
97 return ENOMEM;
98
99 mac48_decode(hdr->src, &frame->src);
100 mac48_decode(hdr->dest, &frame->dest);
101 frame->etype_len = uint16_t_be2host(hdr->etype_len);
102
103 memcpy(frame->data, (uint8_t *)data + sizeof(eth_header_t),
104 frame->size);
105
106 log_msg(LVL_DEBUG, "Ethernet frame src=%llx dest=%llx etype=%x",
107 frame->src, frame->dest, frame->etype_len);
108 log_msg(LVL_DEBUG, "Ethernet frame payload (%zu bytes)", frame->size);
109 size_t i;
110 for (i = 0; i < frame->size; i++) {
111 printf("%02x ", ((uint8_t *)(frame->data))[i]);
112 }
113 printf("\n");
114
115 return EOK;
116}
117
118static void mac48_encode(mac48_addr_t *addr, void *buf)
119{
120 uint64_t val;
121 uint8_t *bbuf = (uint8_t *)buf;
122 int i;
123
124 val = addr->addr;
125 for (i = 0; i < MAC48_BYTES; i++)
126 bbuf[i] = (val >> (8 * (MAC48_BYTES - i - 1))) & 0xff;
127}
128
129static void mac48_decode(void *data, mac48_addr_t *addr)
130{
131 uint64_t val;
132 uint8_t *bdata = (uint8_t *)data;
133 int i;
134
135 val = 0;
136 for (i = 0; i < MAC48_BYTES; i++)
137 val |= (uint64_t)bdata[i] << (8 * (MAC48_BYTES - i - 1));
138
139 addr->addr = val;
140}
141
142/** Encode ARP PDU. */
143int arp_pdu_encode(arp_eth_packet_t *packet, void **rdata, size_t *rsize)
144{
145 void *data;
146 size_t size;
147 arp_eth_packet_fmt_t *pfmt;
148 uint16_t fopcode;
149
150 log_msg(LVL_DEBUG, "arp_pdu_encode()");
151
152 size = sizeof(arp_eth_packet_fmt_t);
153
154 data = calloc(size, 1);
155 if (data == NULL)
156 return ENOMEM;
157
158 pfmt = (arp_eth_packet_fmt_t *)data;
159
160 switch (packet->opcode) {
161 case aop_request: fopcode = AOP_REQUEST; break;
162 case aop_reply: fopcode = AOP_REPLY; break;
163 default:
164 assert(false);
165 fopcode = 0;
166 }
167
168 pfmt->hw_addr_space = host2uint16_t_be(AHRD_ETHERNET);
169 pfmt->proto_addr_space = host2uint16_t_be(ETYPE_IP);
170 pfmt->hw_addr_size = ETH_ADDR_SIZE;
171 pfmt->proto_addr_size = IPV4_ADDR_SIZE;
172 pfmt->opcode = host2uint16_t_be(fopcode);
173 mac48_encode(&packet->sender_hw_addr, pfmt->sender_hw_addr);
174 pfmt->sender_proto_addr =
175 host2uint32_t_be(packet->sender_proto_addr.ipv4);
176 mac48_encode(&packet->target_hw_addr, pfmt->target_hw_addr);
177 pfmt->target_proto_addr =
178 host2uint32_t_be(packet->target_proto_addr.ipv4);
179
180 *rdata = data;
181 *rsize = size;
182 return EOK;
183}
184
185/** Decode ARP PDU. */
186int arp_pdu_decode(void *data, size_t size, arp_eth_packet_t *packet)
187{
188 arp_eth_packet_fmt_t *pfmt;
189
190 log_msg(LVL_DEBUG, "arp_pdu_decode()");
191
192 if (size < sizeof(arp_eth_packet_fmt_t)) {
193 log_msg(LVL_DEBUG, "ARP PDU too short (%zu)", size);
194 return EINVAL;
195 }
196
197 pfmt = (arp_eth_packet_fmt_t *)data;
198
199 if (uint16_t_be2host(pfmt->hw_addr_space) != AHRD_ETHERNET) {
200 log_msg(LVL_DEBUG, "HW address space != %u (%" PRIu16 ")",
201 AHRD_ETHERNET, uint16_t_be2host(pfmt->hw_addr_space));
202 return EINVAL;
203 }
204
205 if (uint16_t_be2host(pfmt->proto_addr_space) != ETYPE_IP) {
206 log_msg(LVL_DEBUG, "Proto address space != %u (%" PRIu16 ")",
207 ETYPE_IP, uint16_t_be2host(pfmt->proto_addr_space));
208 return EINVAL;
209 }
210
211 if (pfmt->hw_addr_size != ETH_ADDR_SIZE) {
212 log_msg(LVL_DEBUG, "HW address size != %zu (%zu)",
213 (size_t)ETH_ADDR_SIZE, (size_t)pfmt->hw_addr_size);
214 return EINVAL;
215 }
216
217 if (pfmt->proto_addr_size != IPV4_ADDR_SIZE) {
218 log_msg(LVL_DEBUG, "Proto address size != %zu (%zu)",
219 (size_t)IPV4_ADDR_SIZE, (size_t)pfmt->proto_addr_size);
220 return EINVAL;
221 }
222
223 switch (uint16_t_be2host(pfmt->opcode)) {
224 case AOP_REQUEST: packet->opcode = aop_request; break;
225 case AOP_REPLY: packet->opcode = aop_reply; break;
226 default:
227 log_msg(LVL_DEBUG, "Invalid ARP opcode (%" PRIu16 ")",
228 uint16_t_be2host(pfmt->opcode));
229 return EINVAL;
230 }
231
232 mac48_decode(pfmt->sender_hw_addr, &packet->sender_hw_addr);
233 packet->sender_proto_addr.ipv4 =
234 uint32_t_be2host(pfmt->sender_proto_addr);
235 mac48_decode(pfmt->target_hw_addr, &packet->target_hw_addr);
236 packet->target_proto_addr.ipv4 =
237 uint32_t_be2host(pfmt->target_proto_addr);
238 log_msg(LVL_DEBUG, "packet->tpa = %x\n", pfmt->target_proto_addr);
239
240 return EOK;
241}
242
243
244/** @}
245 */
Note: See TracBrowser for help on using the repository browser.