mirror of
https://github.com/cesanta/mongoose.git
synced 2026-10-04 23:24:10 +07:00
219 lines
6.9 KiB
C
219 lines
6.9 KiB
C
#include "l2.h"
|
|
#include "log.h"
|
|
#include "net.h"
|
|
#include "net_builtin.h"
|
|
#include "printf.h"
|
|
#include "util.h"
|
|
|
|
#if MG_ENABLE_TCPIP
|
|
|
|
#if defined(__DCC__)
|
|
#pragma pack(1)
|
|
#else
|
|
#pragma pack(push, 1)
|
|
#endif
|
|
|
|
struct eth {
|
|
uint8_t dst[6]; // Destination MAC address
|
|
uint8_t src[6]; // Source MAC address
|
|
uint16_t type; // Ethernet type
|
|
};
|
|
|
|
struct qtag {
|
|
uint16_t tpid; // Ethernet 802.1Q type
|
|
uint16_t tci; // PCP, DEI, VLAN id
|
|
#define VLAN_ID(x) ((uint16_t) ((x) &0x0FFF))
|
|
};
|
|
|
|
#if defined(__DCC__)
|
|
#pragma pack(0)
|
|
#else
|
|
#pragma pack(pop)
|
|
#endif
|
|
|
|
static const uint16_t eth_types[] = {
|
|
// order is vital, see l2.h
|
|
0x800, // IPv4
|
|
0x86dd, // IPv6
|
|
0x806, // ARP
|
|
0x8863, // PPPoE Discovery Stage
|
|
0x8864 // PPPoE Session Stage
|
|
};
|
|
|
|
void mg_l2_eth_init(struct mg_tcpip_if *ifp) {
|
|
struct mg_l2addr *l2addr = (struct mg_l2addr *) ifp->mac;
|
|
// If MAC is not set, make a random one
|
|
if (l2addr->addr.mac[0] == 0 && l2addr->addr.mac[1] == 0 &&
|
|
l2addr->addr.mac[2] == 0 && l2addr->addr.mac[3] == 0 &&
|
|
l2addr->addr.mac[4] == 0 && l2addr->addr.mac[5] == 0) {
|
|
l2addr->addr.mac[0] = 0x02; // Locally administered, unicast
|
|
mg_random(&l2addr->addr.mac[1], sizeof(l2addr->addr.mac) - 1);
|
|
MG_INFO(
|
|
("MAC not set. Generated random: %M", mg_print_mac, l2addr->addr.mac));
|
|
}
|
|
ifp->l2mtu = 1500;
|
|
ifp->framesize = 1540;
|
|
}
|
|
|
|
bool mg_l2_eth_poll(struct mg_tcpip_if *ifp, bool expired_1000ms) {
|
|
(void) ifp;
|
|
(void) expired_1000ms;
|
|
return true;
|
|
}
|
|
|
|
uint8_t *mg_l2_eth_header(struct mg_tcpip_if *ifp, enum mg_l2proto proto,
|
|
struct mg_l2addr *src, struct mg_l2addr *dst,
|
|
uint8_t *frame) {
|
|
struct eth_data *d = &ifp->l2data.eth;
|
|
struct eth *eth = (struct eth *) frame;
|
|
uint8_t *hlp;
|
|
hlp = (uint8_t *) (eth + 1);
|
|
memcpy(eth->src, src->addr.mac, sizeof(eth->dst));
|
|
memcpy(eth->dst, dst->addr.mac, sizeof(eth->dst));
|
|
if (d->vlan_id == 0) { // Traditional plain frame
|
|
eth->type = mg_htons(eth_types[(unsigned int) proto]);
|
|
} else { // Add 802.1Q tag
|
|
struct qtag *qtag = (struct qtag *) ð->type;
|
|
qtag->tpid = mg_htons(0x8100);
|
|
qtag->tci = mg_htons(d->vlan_id); // PCP = default (best-effort)
|
|
hlp += sizeof(*qtag);
|
|
MG_STORE_BE16(qtag + 1, eth_types[(unsigned int) proto]);
|
|
}
|
|
return hlp;
|
|
}
|
|
|
|
size_t mg_l2_eth_trailer(struct mg_tcpip_if *ifp, size_t len, uint8_t *cur) {
|
|
struct eth_data *d = &ifp->l2data.eth;
|
|
// there is no len field in Ethernet, CRC is hw-calculated; pad to 64 - CRC
|
|
size_t ethlen =
|
|
len + sizeof(struct eth) + (d->vlan_id != 0 ? sizeof(struct qtag) : 0);
|
|
(void) cur;
|
|
return ethlen >= 60 ? ethlen : 60;
|
|
}
|
|
|
|
struct mg_l2addr *mg_l2_eth_mapip(enum mg_l2addrtype addrtype,
|
|
struct mg_addr *addr);
|
|
|
|
// Read an unaligned little-endian value from byte pointer p into a native integer.
|
|
// Safe on architectures that forbid unaligned access (e.g. Cortex-M0).
|
|
#define MG_LOAD_LE32(p) \
|
|
((uint32_t) (((uint32_t) MG_U8P(p)[3] << 24U) | \
|
|
((uint32_t) MG_U8P(p)[2] << 16U) | \
|
|
((uint32_t) MG_U8P(p)[1] << 8U) | MG_U8P(p)[0]))
|
|
|
|
bool mg_l2_eth_rx(struct mg_tcpip_if *ifp, enum mg_l2proto *proto,
|
|
struct mg_str *pay, struct mg_str *raw) {
|
|
struct eth *eth = (struct eth *) raw->buf;
|
|
struct eth_data *d = &ifp->l2data.eth;
|
|
uint16_t type, len;
|
|
unsigned int i;
|
|
size_t hdrlen =
|
|
sizeof(struct eth) + (d->vlan_id != 0 ? sizeof(struct qtag) : 0);
|
|
if (raw->len < hdrlen) return false; // Truncated - runt?
|
|
len = (uint16_t) raw->len;
|
|
if (d->vlan_id == 0) { // We don't handle VLANs
|
|
type = mg_ntohs(eth->type);
|
|
} else { // We do, check 802.1Q tag
|
|
struct qtag *qtag = (struct qtag *) ð->type;
|
|
if (qtag->tpid != mg_htons(0x8100)) return false; // Untagged frame
|
|
if (VLAN_ID(mg_ntohs(qtag->tci)) != VLAN_ID(d->vlan_id))
|
|
return false; // Not our VLAN
|
|
type = MG_LOAD_BE16(qtag + 1);
|
|
}
|
|
if (ifp->enable_mac_check &&
|
|
memcmp(eth->dst, ifp->mac, sizeof(eth->dst)) != 0 &&
|
|
memcmp(eth->dst, mg_l2_eth_mapip(MG_TCPIP_L2ADDR_BCAST, NULL),
|
|
sizeof(eth->dst)) != 0)
|
|
return false; // TODO(): add multicast addresses
|
|
if (ifp->enable_fcs_check && len > hdrlen + 4) {
|
|
uint32_t crc, crc_rx;
|
|
len -= 4;
|
|
crc = mg_crc32(0, (const char *) raw->buf, len);
|
|
crc_rx = MG_LOAD_LE32(raw->buf + len);
|
|
if (crc_rx != crc)
|
|
return false;
|
|
}
|
|
pay->buf = ((char *) eth) + hdrlen;
|
|
pay->len = len - hdrlen;
|
|
if (len > ifp->framesize) return false; // Oversized
|
|
for (i = 0; i < sizeof(eth_types) / sizeof(uint16_t); i++) {
|
|
if (type == eth_types[i]) break;
|
|
}
|
|
if (i == sizeof(eth_types) / sizeof(eth_types[0])) {
|
|
MG_DEBUG(("Unknown eth type %x", type));
|
|
if (mg_log_level >= MG_LL_VERBOSE)
|
|
mg_hexdump(raw->buf, raw->len >= 32 ? 32 : raw->len);
|
|
return false;
|
|
}
|
|
*proto = (enum mg_l2proto) i;
|
|
return true;
|
|
}
|
|
|
|
struct mg_l2addr *mg_l2_eth_getaddr(struct mg_tcpip_if *ifp, uint8_t *frame) {
|
|
struct eth *eth = (struct eth *) frame;
|
|
(void) ifp; // address field is before a possible 802.1Q tag
|
|
return (struct mg_l2addr *) ð->src;
|
|
}
|
|
|
|
extern struct mg_l2addr s_mapip;
|
|
|
|
struct mg_l2addr *mg_l2_eth_mapip(enum mg_l2addrtype addrtype,
|
|
struct mg_addr *addr) {
|
|
switch (addrtype) {
|
|
case MG_TCPIP_L2ADDR_BCAST:
|
|
memset(s_mapip.addr.mac, 0xff, sizeof(s_mapip.addr.mac));
|
|
break;
|
|
case MG_TCPIP_L2ADDR_MCAST: {
|
|
uint8_t *ip = (uint8_t *) &addr->addr.ip4;
|
|
// IP multicast group MAC, RFC-1112 6.4
|
|
s_mapip.addr.mac[0] = 0x01, s_mapip.addr.mac[1] = 0x00,
|
|
s_mapip.addr.mac[2] = 0x5E;
|
|
s_mapip.addr.mac[3] = ip[1] & 0x7F; // 23 LSb
|
|
s_mapip.addr.mac[4] = ip[2];
|
|
s_mapip.addr.mac[5] = ip[3];
|
|
break;
|
|
}
|
|
case MG_TCPIP_L2ADDR_MCAST6: {
|
|
// IPv6 multicast address mapping, RFC-2464 7
|
|
uint8_t *ip = (uint8_t *) &addr->addr.ip6;
|
|
s_mapip.addr.mac[0] = 0x33, s_mapip.addr.mac[1] = 0x33;
|
|
s_mapip.addr.mac[2] = ip[12], s_mapip.addr.mac[3] = ip[13],
|
|
s_mapip.addr.mac[4] = ip[14], s_mapip.addr.mac[5] = ip[15];
|
|
break;
|
|
}
|
|
}
|
|
return &s_mapip;
|
|
}
|
|
|
|
#if MG_ENABLE_IPV6
|
|
static void meui64(uint8_t *addr, uint8_t *mac) {
|
|
*addr++ = *mac++ ^ (uint8_t) 0x02, *addr++ = *mac++, *addr++ = *mac++;
|
|
*addr++ = 0xff, *addr++ = 0xfe;
|
|
*addr++ = *mac++, *addr++ = *mac++, *addr = *mac;
|
|
}
|
|
|
|
bool mg_l2_eth_genip6(uint64_t *ip6, uint8_t prefix_len,
|
|
struct mg_l2addr *l2addr) {
|
|
if (prefix_len > 64) {
|
|
MG_ERROR(("Prefix length > 64, UNSUPPORTED"));
|
|
return false;
|
|
}
|
|
ip6[0] = 0;
|
|
meui64(((uint8_t *) &ip6[1]), l2addr->addr.mac); // RFC-4291 2.5.4, 2.5.1
|
|
return true;
|
|
}
|
|
|
|
bool mg_l2_eth_ip6get(struct mg_l2addr *l2addr, uint8_t *opts, uint8_t len) {
|
|
if (len != 1) return false;
|
|
memcpy(l2addr->addr.mac, opts, 6);
|
|
return true;
|
|
}
|
|
|
|
uint8_t mg_l2_eth_ip6put(struct mg_l2addr *l2addr, uint8_t *opts) {
|
|
memcpy(opts, l2addr->addr.mac, 6);
|
|
return 1;
|
|
}
|
|
#endif
|
|
|
|
#endif
|