3party/mongoose
0
0
Fork 0
mirror of https://github.com/cesanta/mongoose.git synced 2024-12-26 22:41:03 +08:00
Code Issues Projects Releases Wiki Activity

Add MIP - an experimental TCP/IP stack

Browse Source
This commit is contained in:
Sergey Lyubka 2022-05-18 21:19:21 +01:00
parent 47e15a3d47
commit 8e0f529a00
10 changed files with 1896 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Makefile
View File

@ -13,7 +13,7 @@ VCFLAGS = /nologo /W3 /O2 /I. $(DEFS) $(TFLAGS)
IPV6 ?= 1
ASAN ?= -fsanitize=address,undefined -fno-sanitize-recover=all
ASAN_OPTIONS ?= detect_leaks=1
EXAMPLES := $(wildcard examples/*)
EXAMPLES := $(dir $(wildcard examples/*/Makefile))
PREFIX ?= /usr/local
VERSION ?= $(shell cut -d'"' -f2 src/version.h)
COMMON_CFLAGS ?= $(WARN) $(INCS) $(DEFS) -DMG_ENABLE_IPV6=$(IPV6) $(TFLAGS) $(EXTRA)
@ -150,8 +150,8 @@ mongoose.c: Makefile $(wildcard src/*)
(cat src/license.h; echo; echo '#include "mongoose.h"' ; (for F in src/*.c ; do echo; echo '#ifdef MG_ENABLE_LINES'; echo "#line 1 \"$$F\""; echo '#endif'; cat $$F | sed -e 's,#include ".*,,'; done))> $@
mongoose.h: $(HDRS) Makefile
(cat src/license.h; echo; echo '#ifndef MONGOOSE_H'; echo '#define MONGOOSE_H'; echo; cat src/version.h ; echo; echo '#ifdef __cplusplus'; echo 'extern "C" {'; echo '#endif'; cat src/arch.h src/arch_*.h src/config.h src/str.h src/log.h src/timer.h src/fs.h src/util.h src/url.h src/iobuf.h src/base64.h src/md5.h src/sha1.h src/event.h src/net.h src/http.h src/ssi.h src/tls.h src/tls_mbed.h src/tls_openssl.h src/ws.h src/sntp.h src/mqtt.h src/dns.h | sed -e 's,#include ".*,,' -e 's,^#pragma once,,'; echo; echo '#ifdef __cplusplus'; echo '}'; echo '#endif'; echo '#endif // MONGOOSE_H')> $@
(cat src/license.h; echo; echo '#ifndef MONGOOSE_H'; echo '#define MONGOOSE_H'; echo; cat src/version.h ; echo; echo '#ifdef __cplusplus'; echo 'extern "C" {'; echo '#endif'; cat src/arch.h src/arch_*.h src/config.h src/str.h src/log.h src/timer.h src/fs.h src/util.h src/url.h src/iobuf.h src/base64.h src/md5.h src/sha1.h src/event.h src/net.h src/http.h src/ssi.h src/tls.h src/tls_mbed.h src/tls_openssl.h src/ws.h src/sntp.h src/mqtt.h src/dns.h src/mip.h | sed -e 's,#include ".*,,' -e 's,^#pragma once,,'; echo; echo '#ifdef __cplusplus'; echo '}'; echo '#endif'; echo '#endif // MONGOOSE_H')> $@
clean:
rm -rf $(PROG) *.o *.dSYM unit_test* valgrind_unit_test* ut fuzzer *.gcov *.gcno *.gcda *.obj *.exe *.ilk *.pdb slow-unit* _CL_* infer-out data.txt crash-* test/packed_fs.c pack unpacked
@for X in $(EXAMPLES); do test -f $$X/Makefile && $(MAKE) -C $$X clean; done
@for X in $(EXAMPLES); do $(MAKE) -C $$X clean; done

2
drivers/README.md Normal file
View File

@ -0,0 +1,2 @@
# This is a collection of low-level drivers for the experimental
# built-in TCP/IP stack

140
drivers/mip_driver_stm32.c Normal file
View File

@ -0,0 +1,140 @@
// Copyright (c) 2022 Cesanta Software Limited
// All rights reserved
#include "mip_driver_stm32.h"
#include <stdint.h>
#include <stdio.h>
#include <string.h>
struct eth {
uint32_t MACCR, MACFFR, MACHTHR, MACHTLR, MACMIIAR, MACMIIDR, MACFCR,
MACVLANTR, RESERVED0[2], MACRWUFFR, MACPMTCSR, RESERVED1, MACDBGR, MACSR,
MACIMR, MACA0HR, MACA0LR, MACA1HR, MACA1LR, MACA2HR, MACA2LR, MACA3HR,
MACA3LR, RESERVED2[40], MMCCR, MMCRIR, MMCTIR, MMCRIMR, MMCTIMR,
RESERVED3[14], MMCTGFSCCR, MMCTGFMSCCR, RESERVED4[5], MMCTGFCR,
RESERVED5[10], MMCRFCECR, MMCRFAECR, RESERVED6[10], MMCRGUFCR,
RESERVED7[334], PTPTSCR, PTPSSIR, PTPTSHR, PTPTSLR, PTPTSHUR, PTPTSLUR,
PTPTSAR, PTPTTHR, PTPTTLR, RESERVED8, PTPTSSR, PTPPPSCR, RESERVED9[564],
DMABMR, DMATPDR, DMARPDR, DMARDLAR, DMATDLAR, DMASR, DMAOMR, DMAIER,
DMAMFBOCR, DMARSWTR, RESERVED10[8], DMACHTDR, DMACHRDR, DMACHTBAR,
DMACHRBAR;
};
#define ETH ((struct eth *) 0x40028000)
#define BIT(x) (1UL << (x))
#define ETH_PKT_SIZE 1540 // Max frame size
#define ETH_DESC_CNT 4 // Descriptors count
#define ETH_DS 4 // Descriptor size (words)
static uint32_t s_rxdesc[ETH_DESC_CNT][ETH_DS]; // RX descriptors
static uint32_t s_txdesc[ETH_DESC_CNT][ETH_DS]; // TX descriptors
static uint8_t s_rxbuf[ETH_DESC_CNT][ETH_PKT_SIZE]; // RX ethernet buffers
static uint8_t s_txbuf[ETH_DESC_CNT][ETH_PKT_SIZE]; // TX ethernet buffers
static void (*s_rx)(void *, size_t, void *); // Recv callback
static void *s_rxdata; // Recv callback data
static void *s_userdata; // Driver data
enum { PHY_ADDR = 0, PHY_BCR = 0, PHY_BSR = 1 }; // PHY constants
static inline void spin(volatile uint32_t count) {
while (count--) asm("nop");
}
uint32_t eth_read_phy(uint8_t addr, uint8_t reg) {
ETH->MACMIIAR &= (7 << 2);
ETH->MACMIIAR |= ((uint32_t) addr << 11) | ((uint32_t) reg << 6);
ETH->MACMIIAR |= BIT(0);
while (ETH->MACMIIAR & BIT(0)) spin(1);
return ETH->MACMIIDR;
}
void eth_write_phy(uint8_t addr, uint8_t reg, uint32_t val) {
ETH->MACMIIDR = val;
ETH->MACMIIAR &= (7 << 2);
ETH->MACMIIAR |= ((uint32_t) addr << 11) | ((uint32_t) reg << 6) | BIT(1);
ETH->MACMIIAR |= BIT(0);
while (ETH->MACMIIAR & BIT(0)) spin(1);
}
void mip_driver_stm32_init(void *userdata) {
s_userdata = userdata;
// Init RX descriptors
for (int i = 0; i < ETH_DESC_CNT; i++) {
s_rxdesc[i][0] = BIT(31); // Own
s_rxdesc[i][1] = sizeof(s_rxbuf[i]) | BIT(14); // 2nd address chained
s_rxdesc[i][2] = (uint32_t) s_rxbuf[i]; // Point to data buffer
s_rxdesc[i][3] = (uint32_t) s_rxdesc[(i + 1) % ETH_DESC_CNT]; // Chain
}
// Init TX descriptors
for (int i = 0; i < ETH_DESC_CNT; i++) {
s_txdesc[i][2] = (uint32_t) s_txbuf[i]; // Buf pointer
s_txdesc[i][3] = (uint32_t) s_txdesc[(i + 1) % ETH_DESC_CNT]; // Chain
}
ETH->DMABMR |= BIT(0); // Software reset
while ((ETH->DMABMR & BIT(0)) != 0) spin(1); // Wait until done
// NOTE(cpq): we do not use extended descriptor bit 7, and do not use
// hardware checksum. Therefore, descriptor size is 4, not 8
// ETH->DMABMR = BIT(13) | BIT(16) | BIT(22) | BIT(23) | BIT(25);
ETH->MACIMR = BIT(3) | BIT(9); // Mask timestamp & PMT IT
ETH->MACMIIAR = 4 << 2; // MDC clock 150-216 MHz, 38.8.1
ETH->MACFCR = BIT(7); // Disable zero quarta pause
ETH->MACFFR = BIT(31); // Receive all
eth_write_phy(PHY_ADDR, PHY_BCR, BIT(15)); // Reset PHY
eth_write_phy(PHY_ADDR, PHY_BCR, BIT(12)); // Set autonegotiation
ETH->DMARDLAR = (uint32_t) s_rxdesc; // RX descriptors
ETH->DMATDLAR = (uint32_t) s_txdesc; // RX descriptors
ETH->DMAIER = BIT(6) | BIT(16); // RIE, NISE
ETH->MACCR = BIT(2) | BIT(3) | BIT(11) | BIT(14); // RE, TE, Duplex, Fast
ETH->DMAOMR = BIT(1) | BIT(13) | BIT(21) | BIT(25); // SR, ST, TSF, RSF
}
void mip_driver_stm32_setrx(void (*rx)(void *, size_t, void *), void *rxdata) {
s_rx = rx;
s_rxdata = rxdata;
}
static uint32_t s_txno;
size_t mip_driver_stm32_tx(const void *buf, size_t len, void *userdata) {
if (len > sizeof(s_txbuf[s_txno])) {
printf("%s: frame too big, %ld\n", __func__, (long) len);
len = 0; // Frame is too big
} else if ((s_txdesc[s_txno][0] & BIT(31))) {
printf("%s: no free descr\n", __func__);
len = 0; // All descriptors are busy, fail
} else {
memcpy(s_txbuf[s_txno], buf, len); // Copy data
s_txdesc[s_txno][1] = (uint32_t) len; // Set data len
s_txdesc[s_txno][0] = BIT(20) | BIT(28) | BIT(29) | BIT(30); // Chain,FS,LS
s_txdesc[s_txno][0] |= BIT(31); // Set OWN bit - let DMA take over
if (++s_txno >= ETH_DESC_CNT) s_txno = 0;
}
uint32_t sr = ETH->DMASR;
if (sr & BIT(2)) ETH->DMASR = BIT(2), ETH->DMATPDR = 0; // Resume
if (sr & BIT(5)) ETH->DMASR = BIT(5), ETH->DMATPDR = 0; // if busy
if (len == 0) printf("E: D0 %lx, DMASR %lx\n", s_txdesc[0][0], sr);
return len;
(void) userdata;
}
bool mip_driver_stm32_status(void *userdata) {
uint32_t bsr = eth_read_phy(PHY_ADDR, PHY_BSR);
return bsr & BIT(2) ? 1 : 0;
(void) userdata;
}
void ETH_IRQHandler(void) {
volatile uint32_t sr = ETH->DMASR;
if (sr & BIT(6)) { // Frame received, loop
for (uint32_t i = 0; i < ETH_DESC_CNT; i++) {
if (s_rxdesc[i][0] & BIT(31)) continue;
uint32_t len = ((s_rxdesc[i][0] >> 16) & (BIT(14) - 1));
// printf("%lx %lu %lx %lx\n", i, len, s_rxdesc[i][0], sr);
if (s_rx != NULL) s_rx(s_rxbuf[i], len > 4 ? len - 4 : len, s_rxdata);
s_rxdesc[i][0] = BIT(31);
}
}
if (sr & BIT(7)) ETH->DMARPDR = 0; // Resume RX
ETH->DMASR = sr & ~(BIT(2) | BIT(7)); // Clear status
}

12
drivers/mip_driver_stm32.h Normal file
View File

@ -0,0 +1,12 @@
// Copyright (c) 2022 Cesanta Software Limited
// All rights reserved
#pragma once
#include <stdbool.h>
#include <stddef.h>
void mip_driver_stm32_init(void *userdata);
bool mip_driver_stm32_status(void *);
void mip_driver_stm32_setrx(void (*rx)(void *, size_t, void *), void *);
size_t mip_driver_stm32_tx(const void *buf, size_t len, void *);

99
drivers/mip_driver_w5500.h Normal file
View File

@ -0,0 +1,99 @@
// Copyright (c) 2022 Cesanta Software Limited
// All rights reserved
//
// This software is dual-licensed: you can redistribute it and/or modify
// it under the terms of the GNU Affero General Public License version 3 as
// published by the Free Software Foundation. For the terms of this
// license, see http://www.fsf.org/licensing/licenses/agpl-3.0.html
//
// You are free to use this software under the terms of the GNU General
// Public License, but WITHOUT ANY WARRANTY; without even the implied
// warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
// See the GNU General Public License for more details.
//
// Alternatively, you can license this software under a commercial
// license, please contact us at https://cesanta.com/contact.html
#pragma once
struct w5500 {
uint8_t mac[6]; // MAC address
void *spi; // Opaque SPI bus descriptor
uint8_t (*txn)(void *, uint8_t); // SPI transaction
void (*begin)(void *); // SPI begin
void (*end)(void *); // SPI end
};
enum { W5500_CR = 0, W5500_S0 = 1, W5500_TX0 = 2, W5500_RX0 = 3 };
static inline void w5500_txn(struct w5500 *w, uint8_t block, uint16_t addr,
bool wr, void *buf, size_t len) {
uint8_t *p = buf, cmd[] = {(uint8_t) (addr >> 8), (uint8_t) (addr & 255),
(uint8_t) ((block << 3) | (wr ? 4 : 0))};
w->begin(w->spi);
for (size_t i = 0; i < sizeof(cmd); i++) w->txn(w->spi, cmd[i]);
for (size_t i = 0; i < len; i++) {
uint8_t r = w->txn(w->spi, p[i]);
if (!wr) p[i] = r;
}
w->end(w->spi);
}
// clang-format off
static inline void w5500_wn(struct w5500 *w, uint8_t block, uint16_t addr, void *buf, size_t len) { w5500_txn(w, block, addr, true, buf, len); }
static inline void w5500_w1(struct w5500 *w, uint8_t block, uint16_t addr, uint8_t val) { w5500_wn(w, block, addr, &val, 1); }
static inline void w5500_w2(struct w5500 *w, uint8_t block, uint16_t addr, uint16_t val) { uint8_t buf[2] = {(uint8_t) (val >> 8), (uint8_t) (val & 255)}; w5500_wn(w, block, addr, buf, sizeof(buf)); }
static inline void w5500_rn(struct w5500 *w, uint8_t block, uint16_t addr, void *buf, size_t len) { w5500_txn(w, block, addr, false, buf, len); }
static inline uint8_t w5500_r1(struct w5500 *w, uint8_t block, uint16_t addr) { uint8_t r = 0; w5500_rn(w, block, addr, &r, 1); return r; }
static inline uint16_t w5500_r2(struct w5500 *w, uint8_t block, uint16_t addr) { uint8_t buf[2] = {0, 0}; w5500_rn(w, block, addr, buf, sizeof(buf)); return (uint16_t) ((buf[0] << 8) | buf[1]); }
// clang-format on
static inline uint8_t w5500_status(struct w5500 *w) {
return w5500_r1(w, W5500_CR, 0x2e);
}
static inline uint16_t w5500_rx(struct w5500 *w, uint8_t *buf, uint16_t len) {
uint16_t r = 0, n = 0, n2; // Read recv len
while ((n2 = w5500_r2(w, W5500_S0, 0x26)) > n) n = n2; // Until it is stable
// printf("RSR: %d\n", (int) n);
if (n > 0) {
uint16_t ptr = w5500_r2(w, W5500_S0, 0x28); // Get read pointer
n = w5500_r2(w, W5500_RX0, ptr); // Read frame length
if (n <= len + 2) r = n - 2, w5500_rn(w, W5500_RX0, ptr + 2, buf, r);
w5500_w2(w, W5500_S0, 0x28, ptr + n); // Advance read pointer
w5500_w1(w, W5500_S0, 1, 0x40); // Sock0 CR -> RECV
// printf(" RX_RD: tot=%u n=%u r=%u\n", n2, n, r);
}
return r;
}
static inline uint16_t w5500_tx(struct w5500 *w, void *buf, uint16_t len) {
uint16_t n = 0;
while (n < len) n = w5500_r2(w, W5500_S0, 0x20); // Wait for space
uint16_t ptr = w5500_r2(w, W5500_S0, 0x24); // Get write pointer
w5500_wn(w, W5500_TX0, ptr, buf, len); // Write data
w5500_w2(w, W5500_S0, 0x24, ptr + len); // Advance write pointer
w5500_w1(w, W5500_S0, 1, 0x20); // Sock0 CR -> SEND
for (int i = 0; i < 40; i++) {
uint8_t ir = w5500_r1(w, W5500_S0, 2); // Read S0 IR
if (ir == 0) continue;
// printf("IR %d, len=%d, free=%d, ptr %d\n", ir, (int) len, (int) n, ptr);
w5500_w1(w, W5500_S0, 2, ir); // Write S0 IR: clear it!
if (ir & 8) len = 0; // Timeout. Report error
if (ir & (16 | 8)) break; // Stop on SEND_OK or timeout
}
return len;
}
static inline bool w5500_init(struct w5500 *w) {
w->end(w->spi);
w5500_w1(w, W5500_CR, 0, 0x80); // Reset chip: CR -> 0x80
w5500_w1(w, W5500_CR, 0x2e, 0); // CR PHYCFGR -> reset
w5500_w1(w, W5500_CR, 0x2e, 0xf8); // CR PHYCFGR -> set
// w5500_wn(w, W5500_CR, 9, w->mac, 6); // Set source MAC
w5500_w1(w, W5500_S0, 0x1e, 16); // Sock0 RX buf size
w5500_w1(w, W5500_S0, 0x1f, 16); // Sock0 TX buf size
w5500_w1(w, W5500_S0, 0, 4); // Sock0 MR -> MACRAW
w5500_w1(w, W5500_S0, 1, 1); // Sock0 CR -> OPEN
return w5500_r1(w, W5500_S0, 3) == 0x42; // Sock0 SR == MACRAW
}

797
mongoose.c
View File

@ -2294,6 +2294,803 @@ void mg_md5_final(mg_md5_ctx *ctx, unsigned char digest[16]) {
}
#endif
#ifdef MG_ENABLE_LINES
#line 1 "src/mip.c"
#endif
#if MG_ENABLE_MIP
#include <stdatomic.h>
#define MIP_ETHEMERAL_PORT 49152
#define _packed __attribute__((packed))
#define U16(ptr) ((((uint16_t) (ptr)[0]) << 8) | (ptr)[1])
#define NET16(x) __builtin_bswap16(x)
#define NET32(x) __builtin_bswap32(x)
#define PDIFF(a, b) ((size_t) (((char *) (b)) - ((char *) (a))))
#ifndef MIP_ARP_ENTRIES
#define MIP_ARP_ENTRIES 5 // Number of ARP cache entries. Maximum 21
#endif
#define MIP_ARP_CS (2 + 12 * MIP_ARP_ENTRIES) // ARP cache size
struct str {
uint8_t *buf;
size_t len;
};
// Receive queue - single producer, single consumer queue. Interrupt-based
// drivers copy received frames to the queue in interrupt context. mip_poll()
// function runs in event loop context, reads from the queue
struct queue {
uint8_t *buf;
size_t len;
volatile _Atomic size_t tail, head;
};
// Network interface
struct mip_if {
uint8_t mac[6]; // MAC address. Must be set to a valid MAC
uint32_t ip, mask, gw; // IP address, mask, default gateway. Can be 0
struct str rx; // Output (TX) buffer
struct str tx; // Input (RX) buffer
bool use_dhcp; // Enable DCHP
struct mip_driver *driver; // Low level driver
struct mg_mgr *mgr; // Mongoose event manager
// Internal state, user can use it but should not change it
uint64_t curtime; // Last poll timestamp in millis
uint64_t timer; // Timer
uint8_t arp_cache[MIP_ARP_CS]; // Each entry is 12 bytes
uint16_t eport; // Next ephemeral port
int state; // Current state
#define MIP_STATE_DOWN 0 // Interface is down
#define MIP_STATE_UP 1 // Interface is up
#define MIP_STATE_READY 2 // Interface is up and has IP
struct queue queue; // Receive queue
};
struct lcp {
uint8_t addr, ctrl, proto[2], code, id, len[2];
} _packed;
struct eth {
uint8_t dst[6]; // Destination MAC address
uint8_t src[6]; // Source MAC address
uint16_t type; // Ethernet type
} _packed;
struct ip {
uint8_t ver; // Version
uint8_t tos; // Unused
uint16_t len; // Length
uint16_t id; // Unused
uint16_t frag; // Fragmentation
uint8_t ttl; // Time to live
uint8_t proto; // Upper level protocol
uint16_t csum; // Checksum
uint32_t src; // Source IP
uint32_t dst; // Destination IP
} _packed;
struct ip6 {
uint8_t ver; // Version
uint8_t opts[3]; // Options
uint16_t len; // Length
uint8_t proto; // Upper level protocol
uint8_t ttl; // Time to live
uint8_t src[16]; // Source IP
uint8_t dst[16]; // Destination IP
} _packed;
struct icmp {
uint8_t type;
uint8_t code;
uint16_t csum;
} _packed;
struct arp {
uint16_t fmt; // Format of hardware address
uint16_t pro; // Format of protocol address
uint8_t hlen; // Length of hardware address
uint8_t plen; // Length of protocol address
uint16_t op; // Operation
uint8_t sha[6]; // Sender hardware address
uint32_t spa; // Sender protocol address
uint8_t tha[6]; // Target hardware address
uint32_t tpa; // Target protocol address
} _packed;
struct tcp {
uint16_t sport; // Source port
uint16_t dport; // Destination port
uint32_t seq; // Sequence number
uint32_t ack; // Acknowledgement number
uint8_t off; // Data offset
uint8_t flags; // TCP flags
#define TH_FIN 0x01
#define TH_SYN 0x02
#define TH_RST 0x04
#define TH_PUSH 0x08
#define TH_ACK 0x10
#define TH_URG 0x20
#define TH_ECE 0x40
#define TH_CWR 0x80
uint16_t win; // Window
uint16_t csum; // Checksum
uint16_t urp; // Urgent pointer
} _packed;
struct udp {
uint16_t sport; // Source port
uint16_t dport; // Destination port
uint16_t len; // UDP length
uint16_t csum; // UDP checksum
} _packed;
struct dhcp {
uint8_t op, htype, hlen, hops;
uint32_t xid;
uint16_t secs, flags;
uint32_t ciaddr, yiaddr, siaddr, giaddr;
uint8_t hwaddr[208];
uint32_t magic;
uint8_t options[32];
} _packed;
struct pkt {
struct str raw; // Raw packet data
struct str pay; // Payload data
struct eth *eth;
struct llc *llc;
struct arp *arp;
struct ip *ip;
struct ip6 *ip6;
struct icmp *icmp;
struct tcp *tcp;
struct udp *udp;
struct dhcp *dhcp;
};
static void q_copyin(struct queue *q, const uint8_t *buf, size_t len,
size_t head) {
size_t i = 0, left = q->len - head;
for (; i < len && i < left; i++) q->buf[head + i] = buf[i];
for (; i < len; i++) q->buf[i - left] = buf[i];
}
static void q_copyout(struct queue *q, uint8_t *buf, size_t len, size_t tail) {
size_t i = 0, left = q->len - tail;
for (; i < len && i < left; i++) buf[i] = q->buf[tail + i];
for (; i < len; i++) buf[i] = q->buf[i - left];
}
static bool q_write(struct queue *q, const void *buf, size_t len) {
bool success = false;
size_t left = q->len - q->head + q->tail;
if (len + sizeof(size_t) <= left) {
q_copyin(q, (uint8_t *) &len, sizeof(len), q->head);
q_copyin(q, (uint8_t *) buf, len, (q->head + sizeof(size_t)) % q->len);
q->head = (q->head + sizeof(len) + len) % q->len;
success = true;
}
return success;
}
static size_t q_avail(struct queue *q) {
size_t n = 0;
if (q->tail != q->head) q_copyout(q, (uint8_t *) &n, sizeof(n), q->tail);
return n;
}
static size_t q_read(struct queue *q, void *buf) {
size_t n = q_avail(q);
if (n > 0) {
q_copyout(q, (uint8_t *) buf, n, (q->tail + sizeof(n)) % q->len);
q->tail = (q->tail + sizeof(n) + n) % q->len;
}
return n;
}
static struct str mkstr(void *buf, size_t len) {
struct str str = {(uint8_t *) buf, len};
return str;
}
static void mkpay(struct pkt *pkt, void *p) {
pkt->pay = mkstr(p, (size_t) (&pkt->raw.buf[pkt->raw.len] - (uint8_t *) p));
}
static uint32_t csumup(uint32_t sum, const void *buf, size_t len) {
const uint8_t *p = (const uint8_t *) buf;
for (size_t i = 0; i < len; i++) sum += i & 1 ? p[i] : (uint32_t) (p[i] << 8);
return sum;
}
static uint16_t csumfin(uint32_t sum) {
while (sum >> 16) sum = (sum & 0xffff) + (sum >> 16);
return NET16(~sum & 0xffff);
}
static uint16_t ipcsum(const void *buf, size_t len) {
uint32_t sum = csumup(0, buf, len);
return csumfin(sum);
}
// ARP cache is organised as a doubly linked list. A successful cache lookup
// moves an entry to the head of the list. New entries are added by replacing
// the last entry in the list with a new IP/MAC.
// ARP cache format: | prev | next | Entry0 | Entry1 | .... | EntryN |
// ARP entry format: | prev | next | IP (4bytes) | MAC (6bytes) |
// prev and next are 1-byte offsets in the cache, so cache size is max 256 bytes
// ARP entry size is 12 bytes
static void arp_cache_init(uint8_t *p, int n, int size) {
for (int i = 0; i < n; i++) p[2 + i * size] = (uint8_t) (2 + (i - 1) * size);
for (int i = 0; i < n; i++) p[3 + i * size] = (uint8_t) (2 + (i + 1) * size);
p[0] = p[2] = (uint8_t) (2 + (n - 1) * size);
p[1] = p[3 + (n - 1) * size] = 2;
}
static uint8_t *arp_cache_find(struct mip_if *ifp, uint32_t ip) {
uint8_t *p = ifp->arp_cache;
if (ip == 0) return NULL;
if (p[0] == 0 || p[1] == 0) arp_cache_init(p, MIP_ARP_ENTRIES, 12);
for (uint8_t i = 0, j = p[1]; i < MIP_ARP_ENTRIES; i++, j = p[j + 1]) {
if (memcmp(p + j + 2, &ip, sizeof(ip)) == 0) {
p[1] = j, p[0] = p[j]; // Found entry! Point list head to us
// MG_DEBUG(("ARP find: %#lx @ %x:%x:%x:%x:%x:%x\n", (long) ip, p[j + 6],
// p[j + 7], p[j + 8], p[j + 9], p[j + 10], p[j + 11]));
return p + j + 6; // And return MAC address
}
}
return NULL;
}
static void arp_cache_add(struct mip_if *ifp, uint32_t ip, uint8_t mac[6]) {
uint8_t *p = ifp->arp_cache;
if (ip == 0 || ip == ~0U) return; // Bad IP
if (arp_cache_find(ifp, ip) != NULL) return; // Already exists, do nothing
memcpy(p + p[0] + 2, &ip, sizeof(ip)); // Replace last entry: IP address
memcpy(p + p[0] + 6, mac, 6); // And MAC address
p[1] = p[0], p[0] = p[p[1]]; // Point list head to us
// MG_DEBUG(("ARP cache: added %#lx @ %x:%x:%x:%x:%x:%x\n", (long) ip, mac[0],
// mac[1], mac[2], mac[3], mac[4], mac[5]));
}
static void arp_ask(struct mip_if *ifp, uint32_t ip) {
struct eth *eth = (struct eth *) ifp->tx.buf;
struct arp *arp = (struct arp *) (eth + 1);
memset(eth->dst, 255, sizeof(eth->dst));
memcpy(eth->src, ifp->mac, sizeof(eth->src));
eth->type = NET16(0x806);
memset(arp, 0, sizeof(*arp));
arp->fmt = NET16(1), arp->pro = NET16(0x800), arp->hlen = 6, arp->plen = 4;
arp->op = NET16(1), arp->tpa = ip, arp->spa = ifp->ip;
memcpy(arp->sha, ifp->mac, sizeof(arp->sha));
ifp->driver->tx(eth, PDIFF(eth, arp + 1), ifp->driver->data);
}
static void onstatechange(struct mip_if *ifp) {
if (ifp->state == MIP_STATE_READY) {
char buf[40];
struct mg_addr addr = {.ip = ifp->ip};
MG_INFO(("READY, IP: %s", mg_ntoa(&addr, buf, sizeof(buf))));
arp_ask(ifp, ifp->gw);
} else if (ifp->state == MIP_STATE_UP) {
MG_ERROR(("Network up"));
} else if (ifp->state == MIP_STATE_DOWN) {
MG_ERROR(("Network down"));
}
}
static struct ip *tx_ip(struct mip_if *ifp, uint8_t proto, uint32_t ip_src,
uint32_t ip_dst, size_t plen) {
struct eth *eth = (struct eth *) ifp->tx.buf;
struct ip *ip = (struct ip *) (eth + 1);
uint8_t *mac = arp_cache_find(ifp, ip_dst); // Dst IP in ARP cache ?
if (!mac) mac = arp_cache_find(ifp, ifp->gw); // No, use gateway
if (mac) memcpy(eth->dst, mac, sizeof(eth->dst)); // Found? Use it
if (!mac) memset(eth->dst, 255, sizeof(eth->dst)); // No? Use broadcast
memcpy(eth->src, ifp->mac, sizeof(eth->src)); // TODO(cpq): ARP lookup
eth->type = NET16(0x800);
memset(ip, 0, sizeof(*ip));
ip->ver = 0x45; // Version 4, header length 5 words
ip->frag = 0x40; // Don't fragment
ip->len = NET16((uint16_t) (sizeof(*ip) + plen));
ip->ttl = 64;
ip->proto = proto;
ip->src = ip_src;
ip->dst = ip_dst;
ip->csum = ipcsum(ip, sizeof(*ip));
return ip;
}
void tx_udp(struct mip_if *ifp, uint32_t ip_src, uint16_t sport,
uint32_t ip_dst, uint16_t dport, const void *buf, size_t len) {
struct ip *ip = tx_ip(ifp, 17, ip_src, ip_dst, len + sizeof(struct udp));
struct udp *udp = (struct udp *) (ip + 1);
udp->sport = sport;
udp->dport = dport;
udp->len = NET16((uint16_t) (sizeof(*udp) + len));
udp->csum = 0;
uint32_t cs = csumup(0, udp, sizeof(*udp));
cs = csumup(cs, buf, len);
cs = csumup(cs, &ip->src, sizeof(ip->src));
cs = csumup(cs, &ip->dst, sizeof(ip->dst));
cs += ip->proto + sizeof(*udp) + len;
udp->csum = csumfin(cs);
memmove(udp + 1, buf, len);
// MG_DEBUG(("UDP LEN %d %d\n", (int) len, (int) ifp->frame_len));
ifp->driver->tx(ifp->tx.buf,
sizeof(struct eth) + sizeof(*ip) + sizeof(*udp) + len,
ifp->driver->data);
}
static void tx_dhcp(struct mip_if *ifp, uint32_t src, uint32_t dst,
uint8_t *opts, size_t optslen) {
struct dhcp dhcp = {.op = 1,
.htype = 1,
.hlen = 6,
.ciaddr = src,
.magic = NET32(0x63825363)};
memcpy(&dhcp.hwaddr, ifp->mac, sizeof(ifp->mac));
memcpy(&dhcp.xid, ifp->mac + 2, sizeof(dhcp.xid));
memcpy(&dhcp.options, opts, optslen);
tx_udp(ifp, src, NET16(68), dst, NET16(67), &dhcp, sizeof(dhcp));
}
static void tx_dhcp_request(struct mip_if *ifp, uint32_t src, uint32_t dst) {
uint8_t opts[] = {
53, 1, 3, // Type: DHCP request
55, 2, 1, 3, // GW and mask
12, 3, 'm', 'i', 'p', // Host name: "mip"
54, 4, 0, 0, 0, 0, // DHCP server ID
50, 4, 0, 0, 0, 0, // Requested IP
255 // End of options
};
memcpy(opts + 14, &dst, sizeof(dst));
memcpy(opts + 20, &src, sizeof(src));
tx_dhcp(ifp, src, dst, opts, sizeof(opts));
}
static void tx_dhcp_discover(struct mip_if *ifp) {
uint8_t opts[] = {
53, 1, 1, // Type: DHCP discover
55, 2, 1, 3, // Parameters: ip, mask
255 // End of options
};
tx_dhcp(ifp, 0, 0xffffffff, opts, sizeof(opts));
}
static void rx_arp(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("ARP op %d %#x %#x\n", NET16(arp->op), arp->spa, arp->tpa));
if (pkt->arp->op == NET16(1) && pkt->arp->tpa == ifp->ip) {
// ARP request. Make a response, then send
struct eth *eth = (struct eth *) ifp->tx.buf;
struct arp *arp = (struct arp *) (eth + 1);
memcpy(eth->dst, pkt->eth->src, sizeof(eth->dst));
memcpy(eth->src, ifp->mac, sizeof(eth->src));
eth->type = NET16(0x806);
*arp = *pkt->arp;
arp->op = NET16(2);
memcpy(arp->tha, pkt->arp->sha, sizeof(pkt->arp->tha));
memcpy(arp->sha, ifp->mac, sizeof(pkt->arp->sha));
arp->tpa = pkt->arp->spa;
arp->spa = ifp->ip;
MG_DEBUG(("ARP response: we're %#lx", (long) ifp->ip));
ifp->driver->tx(ifp->tx.buf, PDIFF(eth, arp + 1), ifp->driver->data);
} else if (pkt->arp->op == NET16(2)) {
if (memcmp(pkt->arp->tha, ifp->mac, sizeof(pkt->arp->tha)) != 0) return;
// MG_INFO(("ARP RESPONSE"));
arp_cache_add(ifp, pkt->arp->spa, pkt->arp->sha);
}
}
static void rx_icmp(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("ICMP %d\n", (int) len));
if (pkt->icmp->type == 8 && pkt->ip->dst == ifp->ip) {
struct ip *ip = tx_ip(ifp, 1, ifp->ip, pkt->ip->src,
sizeof(struct icmp) + pkt->pay.len);
struct icmp *icmp = (struct icmp *) (ip + 1);
memset(icmp, 0, sizeof(*icmp)); // Important - set csum to 0
memcpy(icmp + 1, pkt->pay.buf, pkt->pay.len);
icmp->csum = ipcsum(icmp, sizeof(*icmp) + pkt->pay.len);
ifp->driver->tx(ifp->tx.buf, PDIFF(ifp->tx.buf, icmp + 1) + pkt->pay.len,
ifp->driver->data);
}
}
static void rx_dhcp(struct mip_if *ifp, struct pkt *pkt) {
uint32_t ip = 0, gw = 0, mask = 0;
uint8_t *p = pkt->dhcp->options, *end = &pkt->raw.buf[pkt->raw.len];
if (end < (uint8_t *) (pkt->dhcp + 1)) return;
// MG_DEBUG(("DHCP %u\n", (unsigned) pkt->raw.len));
while (p < end && p[0] != 255) {
if (p[0] == 1 && p[1] == sizeof(ifp->mask)) {
memcpy(&mask, p + 2, sizeof(mask));
// MG_DEBUG(("MASK %x\n", mask));
} else if (p[0] == 3 && p[1] == sizeof(ifp->gw)) {
memcpy(&gw, p + 2, sizeof(gw));
ip = pkt->dhcp->yiaddr;
// MG_DEBUG(("IP %x GW %x\n", ip, gw));
}
p += p[1] + 2;
}
if (ip && mask && gw && ifp->ip == 0) {
// MG_DEBUG(("DHCP offer ip %#08lx mask %#08lx gw %#08lx\n",
// (long) ip, (long) mask, (long) gw));
arp_cache_add(ifp, pkt->dhcp->siaddr, ((struct eth *) pkt->raw.buf)->src);
ifp->ip = ip, ifp->gw = gw, ifp->mask = mask;
ifp->state = MIP_STATE_READY;
onstatechange(ifp);
tx_dhcp_request(ifp, ip, pkt->dhcp->siaddr);
}
}
struct mg_connection *getpeer(struct mg_mgr *mgr, struct pkt *pkt, bool lsn) {
struct mg_connection *c = NULL;
for (c = mgr->conns; c != NULL; c = c->next) {
if (c->is_udp && pkt->udp && c->loc.port == pkt->udp->dport) break;
if (!c->is_udp && pkt->tcp && c->loc.port == pkt->tcp->dport &&
lsn == c->is_listening && (lsn || c->rem.port == pkt->tcp->sport))
break;
}
return c;
}
static void rx_udp(struct mip_if *ifp, struct pkt *pkt) {
struct mg_connection *c = getpeer(ifp->mgr, pkt, true);
if (c == NULL) {
// No UDP listener on this port. Should send ICMP, but keep silent.
} else if (c != NULL) {
c->rem.port = pkt->udp->sport;
c->rem.ip = pkt->ip->src;
if (c->recv.len >= MG_MAX_RECV_BUF_SIZE) {
mg_error(c, "max_recv_buf_size reached");
} else if (c->recv.size - c->recv.len < pkt->pay.len &&
!mg_iobuf_resize(&c->recv, c->recv.len + pkt->pay.len)) {
mg_error(c, "oom");
} else {
memcpy(&c->recv.buf[c->recv.len], pkt->pay.buf, pkt->pay.len);
c->recv.len += pkt->pay.len;
struct mg_str evd = mg_str_n((char *) pkt->pay.buf, pkt->pay.len);
mg_call(c, MG_EV_READ, &evd);
}
}
}
struct tcpstate {
uint32_t seq, ack;
time_t expire;
};
static size_t tx_tcp(struct mip_if *ifp, uint32_t dst_ip, uint8_t flags,
uint16_t sport, uint16_t dport, uint32_t seq, uint32_t ack,
const void *buf, size_t len) {
struct ip *ip = tx_ip(ifp, 6, ifp->ip, dst_ip, sizeof(struct tcp) + len);
struct tcp *tcp = (struct tcp *) (ip + 1);
memset(tcp, 0, sizeof(*tcp));
memmove(tcp + 1, buf, len);
tcp->sport = sport;
tcp->dport = dport;
tcp->seq = seq;
tcp->ack = ack;
tcp->flags = flags;
tcp->win = mg_htons(8192);
tcp->off = (uint8_t) (sizeof(*tcp) / 4 << 4);
uint32_t cs = 0;
uint16_t n = (uint16_t) (sizeof(*tcp) + len);
uint8_t pseudo[] = {0, ip->proto, (uint8_t) (n >> 8), (uint8_t) (n & 255)};
cs = csumup(cs, tcp, n);
cs = csumup(cs, &ip->src, sizeof(ip->src));
cs = csumup(cs, &ip->dst, sizeof(ip->dst));
cs = csumup(cs, pseudo, sizeof(pseudo));
tcp->csum = csumfin(cs);
return ifp->driver->tx(ifp->tx.buf, PDIFF(ifp->tx.buf, tcp + 1) + len,
ifp->driver->data);
}
static size_t tx_tcp_pkt(struct mip_if *ifp, struct pkt *pkt, uint8_t flags,
uint32_t seq, const void *buf, size_t len) {
uint32_t delta = (pkt->tcp->flags & (TH_SYN | TH_FIN)) ? 1 : 0;
return tx_tcp(ifp, pkt->ip->src, flags, pkt->tcp->dport, pkt->tcp->sport, seq,
mg_htonl(mg_ntohl(pkt->tcp->seq) + delta), buf, len);
}
static struct mg_connection *accept_conn(struct mg_connection *lsn,
struct pkt *pkt) {
struct mg_connection *c = mg_alloc_conn(lsn->mgr);
struct tcpstate *s = (struct tcpstate *) (c + 1);
s->seq = mg_ntohl(pkt->tcp->ack), s->ack = mg_ntohl(pkt->tcp->seq);
c->rem.ip = pkt->ip->src;
c->rem.port = pkt->tcp->sport;
MG_DEBUG(("%lu accepted %lx:%hx", c->id, c->rem.ip, c->rem.port));
LIST_ADD_HEAD(struct mg_connection, &lsn->mgr->conns, c);
c->fd = (void *) (size_t) mg_ntohl(pkt->tcp->ack);
c->is_accepted = 1;
c->is_hexdumping = lsn->is_hexdumping;
c->pfn = lsn->pfn;
c->loc = lsn->loc;
c->pfn_data = lsn->pfn_data;
c->fn = lsn->fn;
c->fn_data = lsn->fn_data;
mg_call(c, MG_EV_OPEN, NULL);
mg_call(c, MG_EV_ACCEPT, NULL);
return c;
}
static void read_conn(struct mg_connection *c, struct pkt *pkt) {
struct tcpstate *s = (struct tcpstate *) (c + 1);
if (pkt->tcp->flags & TH_FIN) {
s->ack = mg_htonl(pkt->tcp->seq) + 1, s->seq = mg_htonl(pkt->tcp->ack);
c->is_closing = 1;
} else if (pkt->pay.len == 0) {
} else if (c->recv.size - c->recv.len < pkt->pay.len &&
!mg_iobuf_resize(&c->recv, c->recv.len + pkt->pay.len)) {
mg_error(c, "oom");
} else if (mg_ntohl(pkt->tcp->seq) != s->ack) {
mg_error(c, "oob: %x %x", mg_ntohl(pkt->tcp->seq), s->ack);
} else {
s->ack = mg_htonl(pkt->tcp->seq) + pkt->pay.len;
memcpy(&c->recv.buf[c->recv.len], pkt->pay.buf, pkt->pay.len);
c->recv.len += pkt->pay.len;
struct mg_str evd = mg_str_n((char *) pkt->pay.buf, pkt->pay.len);
mg_call(c, MG_EV_READ, &evd);
#if 0
// Send ACK immediately
tx_tcp(ifp, c->rem.ip, TH_ACK, c->loc.port, c->rem.port, mg_htonl(s->seq),
mg_htonl(s->ack), NULL, 0);
#endif
}
}
static void rx_tcp(struct mip_if *ifp, struct pkt *pkt) {
struct mg_connection *c = getpeer(ifp->mgr, pkt, false);
#if 0
MG_INFO(("%lu %hhu %d", c ? c->id : 0, pkt->tcp->flags, (int) pkt->pay.len));
#endif
if (c != NULL) {
#if 0
MG_DEBUG(("%lu %d %lx:%hx -> %lx:%hx", c->id, (int) pkt->raw.len,
pkt->ip->src, pkt->tcp->sport, pkt->ip->dst, pkt->tcp->dport));
hexdump(pkt->pay.buf, pkt->pay.len);
#endif
read_conn(c, pkt);
} else if ((c = getpeer(ifp->mgr, pkt, true)) == NULL) {
tx_tcp_pkt(ifp, pkt, TH_RST | TH_ACK, pkt->tcp->ack, NULL, 0);
} else if (pkt->tcp->flags & TH_SYN) {
// Use peer's source port as ISN, in order to recognise the handshake
uint32_t isn = mg_htonl((uint32_t) mg_ntohs(pkt->tcp->sport));
tx_tcp_pkt(ifp, pkt, TH_SYN | TH_ACK, isn, NULL, 0);
} else if (pkt->tcp->flags & TH_FIN) {
tx_tcp_pkt(ifp, pkt, TH_FIN | TH_ACK, pkt->tcp->ack, NULL, 0);
} else if (mg_htonl(pkt->tcp->ack) == mg_htons(pkt->tcp->sport) + 1U) {
accept_conn(c, pkt);
} else {
// MG_DEBUG(("dropped silently.."));
}
}
static void rx_ip(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("IP %d", (int) pkt->pay.len));
if (pkt->ip->proto == 1) {
pkt->icmp = (struct icmp *) (pkt->ip + 1);
if (pkt->pay.len < sizeof(*pkt->icmp)) return;
mkpay(pkt, pkt->icmp + 1);
rx_icmp(ifp, pkt);
} else if (pkt->ip->proto == 17) {
pkt->udp = (struct udp *) (pkt->ip + 1);
if (pkt->pay.len < sizeof(*pkt->udp)) return;
// MG_DEBUG((" UDP %u %u -> %u\n", len, NET16(udp->sport),
// NET16(udp->dport)));
mkpay(pkt, pkt->udp + 1);
if (pkt->udp->dport == NET16(68)) {
pkt->dhcp = (struct dhcp *) (pkt->udp + 1);
mkpay(pkt, pkt->dhcp + 1);
rx_dhcp(ifp, pkt);
} else {
rx_udp(ifp, pkt);
}
} else if (pkt->ip->proto == 6) {
pkt->tcp = (struct tcp *) (pkt->ip + 1);
if (pkt->pay.len < sizeof(*pkt->tcp)) return;
mkpay(pkt, pkt->tcp + 1);
uint16_t iplen = mg_ntohs(pkt->ip->len);
uint16_t off = (uint16_t) (sizeof(*pkt->ip) + ((pkt->tcp->off >> 4) * 4U));
if (iplen >= off) pkt->pay.len = (size_t) (iplen - off);
rx_tcp(ifp, pkt);
}
}
static void rx_ip6(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("IP %d\n", (int) len));
if (pkt->ip6->proto == 1 || pkt->ip6->proto == 58) {
pkt->icmp = (struct icmp *) (pkt->ip6 + 1);
if (pkt->pay.len < sizeof(*pkt->icmp)) return;
mkpay(pkt, pkt->icmp + 1);
rx_icmp(ifp, pkt);
} else if (pkt->ip->proto == 17) {
pkt->udp = (struct udp *) (pkt->ip6 + 1);
if (pkt->pay.len < sizeof(*pkt->udp)) return;
// MG_DEBUG((" UDP %u %u -> %u\n", len, NET16(udp->sport),
// NET16(udp->dport)));
mkpay(pkt, pkt->udp + 1);
}
}
static void mip_rx(struct mip_if *ifp, void *buf, size_t len) {
const uint8_t broadcast[] = {255, 255, 255, 255, 255, 255};
struct pkt pkt = {.raw = {.buf = (uint8_t *) buf, .len = len}};
pkt.eth = (struct eth *) buf;
if (pkt.raw.len < sizeof(*pkt.eth)) return; // Truncated - runt?
if (memcmp(pkt.eth->dst, ifp->mac, sizeof(pkt.eth->dst)) != 0 &&
memcmp(pkt.eth->dst, broadcast, sizeof(pkt.eth->dst)) != 0) {
// Not for us. Drop silently
} else if (pkt.eth->type == NET16(0x806)) {
pkt.arp = (struct arp *) (pkt.eth + 1);
if (sizeof(*pkt.eth) + sizeof(*pkt.arp) > pkt.raw.len) return; // Truncated
rx_arp(ifp, &pkt);
} else if (pkt.eth->type == NET16(0x86dd)) {
pkt.ip6 = (struct ip6 *) (pkt.eth + 1);
if (pkt.raw.len < sizeof(*pkt.eth) + sizeof(*pkt.ip6)) return; // Truncated
if ((pkt.ip6->ver >> 4) != 0x6) return; // Not IP
mkpay(&pkt, pkt.ip6 + 1);
rx_ip6(ifp, &pkt);
} else if (pkt.eth->type == NET16(0x800)) {
pkt.ip = (struct ip *) (pkt.eth + 1);
if (pkt.raw.len < sizeof(*pkt.eth) + sizeof(*pkt.ip)) return; // Truncated
if ((pkt.ip->ver >> 4) != 4) return; // Not IP
mkpay(&pkt, pkt.ip + 1);
rx_ip(ifp, &pkt);
} else {
MG_DEBUG((" Unknown eth type %x\n", NET16(pkt.eth->type)));
}
}
static void mip_poll(struct mip_if *ifp, uint64_t uptime_ms) {
ifp->curtime = uptime_ms;
if (ifp->ip == 0 && uptime_ms > ifp->timer) {
tx_dhcp_discover(ifp); // If IP not configured, send DHCP
ifp->timer = uptime_ms + 1000; // with some interval
} else if (ifp->use_dhcp == false && uptime_ms > ifp->timer &&
arp_cache_find(ifp, ifp->gw) == NULL) {
arp_ask(ifp, ifp->gw); // If GW's MAC address in not in ARP cache
ifp->timer = uptime_ms + 1000; // send ARP who-has request
}
// Handle physical interface up/down status
if (ifp->driver->status) {
bool up = ifp->driver->status(ifp->driver->data);
bool current = ifp->state != MIP_STATE_DOWN;
if (up != current) {
ifp->state = up == false ? MIP_STATE_DOWN
: ifp->use_dhcp ? MIP_STATE_UP
: MIP_STATE_READY;
if (!up && ifp->use_dhcp) ifp->ip = 0;
onstatechange(ifp);
}
}
// Read data from the network
for (;;) {
size_t len = ifp->queue.len > 0 ? q_read(&ifp->queue, ifp->rx.buf)
: ifp->driver->rx(ifp->rx.buf, ifp->rx.len,
ifp->driver->data);
if (len == 0) break;
mip_rx(ifp, ifp->rx.buf, len);
}
}
// This function executes in interrupt context, thus it should copy data
// somewhere fast. Note that newlib's malloc is not thread safe, thus use
// our lock-free queue with preallocated buffer to copy data and return asap
static void on_rx(void *buf, size_t len, void *userdata) {
struct mip_if *ifp = (struct mip_if *) userdata;
if (!q_write(&ifp->queue, buf, len)) MG_ERROR(("dropped %d", (int) len));
}
void mip_init(struct mg_mgr *mgr, struct mip_ipcfg *ipcfg,
struct mip_driver *driver) {
size_t maxpktsize = 1500, qlen = driver->rxcb ? 1024 * 16 : 0;
struct mip_if *ifp =
(struct mip_if *) calloc(1, sizeof(*ifp) + 2 * maxpktsize + qlen);
memcpy(ifp->mac, ipcfg->mac, sizeof(ifp->mac));
ifp->use_dhcp = ipcfg->ip == 0;
ifp->ip = ipcfg->ip, ifp->mask = ipcfg->mask, ifp->gw = ipcfg->gw;
ifp->rx.buf = (uint8_t *) (ifp + 1), ifp->rx.len = maxpktsize;
ifp->tx.buf = ifp->rx.buf + maxpktsize, ifp->tx.len = maxpktsize;
ifp->driver = driver;
ifp->mgr = mgr;
ifp->queue.buf = ifp->tx.buf + maxpktsize;
ifp->queue.len = qlen;
if (driver->init) driver->init(driver->data);
if (driver->rxcb) driver->rxcb(on_rx, ifp);
mgr->priv = ifp;
mgr->extraconnsize = sizeof(struct tcpstate);
}
void mg_connect_resolved(struct mg_connection *c) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
if (ifp->eport < MIP_ETHEMERAL_PORT) ifp->eport = MIP_ETHEMERAL_PORT;
if (c->is_udp) {
c->loc.ip = ifp->ip;
c->loc.port = mg_htons(ifp->eport++);
MG_DEBUG(("%lu %08lx.%hu->%08lx.%hu", c->id, mg_ntohl(c->loc.ip),
mg_ntohs(c->loc.port), mg_ntohl(c->rem.ip),
mg_ntohs(c->rem.port)));
mg_call(c, MG_EV_RESOLVE, NULL);
mg_call(c, MG_EV_CONNECT, NULL);
} else {
mg_error(c, "Not implemented");
}
c->is_resolving = 0;
}
bool mg_open_listener(struct mg_connection *c, const char *url) {
c->loc.port = mg_htons(mg_url_port(url));
return true;
}
static void write_conn(struct mg_connection *c) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
struct tcpstate *s = (struct tcpstate *) (c + 1);
size_t sent, n = c->send.len, hdrlen = 14 + 24 /*max IP*/ + 60 /*max TCP*/;
if (n + hdrlen > ifp->tx.len) n = ifp->tx.len - hdrlen;
sent = tx_tcp(ifp, c->rem.ip, TH_PUSH | TH_ACK, c->loc.port, c->rem.port,
mg_htonl(s->seq), mg_htonl(s->ack), c->send.buf, n);
if (sent > 0) {
mg_iobuf_del(&c->send, 0, n);
s->seq += n;
mg_call(c, MG_EV_WRITE, &n);
}
}
static void fin_conn(struct mg_connection *c) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
struct tcpstate *s = (struct tcpstate *) (c + 1);
tx_tcp(ifp, c->rem.ip, TH_FIN | TH_ACK, c->loc.port, c->rem.port,
mg_htonl(s->seq), mg_htonl(s->ack), NULL, 0);
}
void mg_mgr_poll(struct mg_mgr *mgr, int ms) {
struct mg_connection *c, *tmp;
uint64_t now = mg_millis();
mip_poll((struct mip_if *) mgr->priv, now);
mg_timer_poll(&mgr->timers, now);
for (c = mgr->conns; c != NULL; c = tmp) {
tmp = c->next;
if (c->send.len > 0) write_conn(c);
if (c->is_draining && c->send.len == 0) c->is_closing = 1;
if (c->is_closing) {
if (c->is_udp == false && c->is_listening == false) fin_conn(c);
mg_close_conn(c);
}
}
(void) ms;
}
bool mg_send(struct mg_connection *c, const void *buf, size_t len) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
bool res = false;
if (ifp->ip == 0) {
mg_error(c, "net down");
} else if (c->is_udp) {
tx_udp(ifp, ifp->ip, c->loc.port, c->rem.ip, c->rem.port, buf, len);
res = true;
} else {
// tx_tdp(ifp, ifp->ip, c->loc.port, c->rem.ip, c->rem.port, buf, len);
return mg_iobuf_add(&c->send, c->send.len, buf, len, MG_IO_SIZE);
}
return res;
}
int mg_mkpipe(struct mg_mgr *mgr, mg_event_handler_t fn, void *fn_data) {
(void) mgr, (void) fn, (void) fn_data;
return -1;
}
#endif // MG_ENABLE_MIP
#ifdef MG_ENABLE_LINES
#line 1 "src/mqtt.c"
#endif

25
mongoose.h
View File

@ -575,6 +575,10 @@ int sscanf(const char *, const char *, ...);
#endif
#ifndef MG_ENABLE_MIP
#define MG_ENABLE_MIP 0
#endif
#ifndef MG_ENABLE_FATFS
#define MG_ENABLE_FATFS 0
#endif
@ -1266,6 +1270,27 @@ bool mg_dns_parse(const uint8_t *buf, size_t len, struct mg_dns_message *);
size_t mg_dns_parse_rr(const uint8_t *buf, size_t len, size_t ofs,
bool is_question, struct mg_dns_rr *);
struct mip_driver {
void *data; // Driver-specific data
void (*init)(void *data); // Initialise driver
size_t (*tx)(const void *, size_t, void *data); // Transmit frame
size_t (*rx)(void *buf, size_t len, void *data); // Receive frame (polling)
bool (*status)(void *data); // Up/down status
// Set receive callback for interrupt-driven drivers
void (*rxcb)(void (*fn)(void *buf, size_t len, void *rxdata), void *rxdata);
};
struct mip_ipcfg {
uint8_t mac[6]; // MAC address. Must not be 0
uint32_t ip, mask, gw; // IP, netmask, GW. If IP is 0, DHCP is used
};
void mip_init(struct mg_mgr *, struct mip_ipcfg *, struct mip_driver *);
#ifdef __cplusplus
}
#endif

4
src/config.h
View File

@ -1,5 +1,9 @@
#pragma once
#ifndef MG_ENABLE_MIP
#define MG_ENABLE_MIP 0
#endif
#ifndef MG_ENABLE_FATFS
#define MG_ENABLE_FATFS 0
#endif

793
src/mip.c Normal file
View File

@ -0,0 +1,793 @@
#include "mip.h"
#include "config.h"
#if MG_ENABLE_MIP
#include <stdatomic.h>
#define MIP_ETHEMERAL_PORT 49152
#define _packed __attribute__((packed))
#define U16(ptr) ((((uint16_t) (ptr)[0]) << 8) | (ptr)[1])
#define NET16(x) __builtin_bswap16(x)
#define NET32(x) __builtin_bswap32(x)
#define PDIFF(a, b) ((size_t) (((char *) (b)) - ((char *) (a))))
#ifndef MIP_ARP_ENTRIES
#define MIP_ARP_ENTRIES 5 // Number of ARP cache entries. Maximum 21
#endif
#define MIP_ARP_CS (2 + 12 * MIP_ARP_ENTRIES) // ARP cache size
struct str {
uint8_t *buf;
size_t len;
};
// Receive queue - single producer, single consumer queue. Interrupt-based
// drivers copy received frames to the queue in interrupt context. mip_poll()
// function runs in event loop context, reads from the queue
struct queue {
uint8_t *buf;
size_t len;
volatile _Atomic size_t tail, head;
};
// Network interface
struct mip_if {
uint8_t mac[6]; // MAC address. Must be set to a valid MAC
uint32_t ip, mask, gw; // IP address, mask, default gateway. Can be 0
struct str rx; // Output (TX) buffer
struct str tx; // Input (RX) buffer
bool use_dhcp; // Enable DCHP
struct mip_driver *driver; // Low level driver
struct mg_mgr *mgr; // Mongoose event manager
// Internal state, user can use it but should not change it
uint64_t curtime; // Last poll timestamp in millis
uint64_t timer; // Timer
uint8_t arp_cache[MIP_ARP_CS]; // Each entry is 12 bytes
uint16_t eport; // Next ephemeral port
int state; // Current state
#define MIP_STATE_DOWN 0 // Interface is down
#define MIP_STATE_UP 1 // Interface is up
#define MIP_STATE_READY 2 // Interface is up and has IP
struct queue queue; // Receive queue
};
struct lcp {
uint8_t addr, ctrl, proto[2], code, id, len[2];
} _packed;
struct eth {
uint8_t dst[6]; // Destination MAC address
uint8_t src[6]; // Source MAC address
uint16_t type; // Ethernet type
} _packed;
struct ip {
uint8_t ver; // Version
uint8_t tos; // Unused
uint16_t len; // Length
uint16_t id; // Unused
uint16_t frag; // Fragmentation
uint8_t ttl; // Time to live
uint8_t proto; // Upper level protocol
uint16_t csum; // Checksum
uint32_t src; // Source IP
uint32_t dst; // Destination IP
} _packed;
struct ip6 {
uint8_t ver; // Version
uint8_t opts[3]; // Options
uint16_t len; // Length
uint8_t proto; // Upper level protocol
uint8_t ttl; // Time to live
uint8_t src[16]; // Source IP
uint8_t dst[16]; // Destination IP
} _packed;
struct icmp {
uint8_t type;
uint8_t code;
uint16_t csum;
} _packed;
struct arp {
uint16_t fmt; // Format of hardware address
uint16_t pro; // Format of protocol address
uint8_t hlen; // Length of hardware address
uint8_t plen; // Length of protocol address
uint16_t op; // Operation
uint8_t sha[6]; // Sender hardware address
uint32_t spa; // Sender protocol address
uint8_t tha[6]; // Target hardware address
uint32_t tpa; // Target protocol address
} _packed;
struct tcp {
uint16_t sport; // Source port
uint16_t dport; // Destination port
uint32_t seq; // Sequence number
uint32_t ack; // Acknowledgement number
uint8_t off; // Data offset
uint8_t flags; // TCP flags
#define TH_FIN 0x01
#define TH_SYN 0x02
#define TH_RST 0x04
#define TH_PUSH 0x08
#define TH_ACK 0x10
#define TH_URG 0x20
#define TH_ECE 0x40
#define TH_CWR 0x80
uint16_t win; // Window
uint16_t csum; // Checksum
uint16_t urp; // Urgent pointer
} _packed;
struct udp {
uint16_t sport; // Source port
uint16_t dport; // Destination port
uint16_t len; // UDP length
uint16_t csum; // UDP checksum
} _packed;
struct dhcp {
uint8_t op, htype, hlen, hops;
uint32_t xid;
uint16_t secs, flags;
uint32_t ciaddr, yiaddr, siaddr, giaddr;
uint8_t hwaddr[208];
uint32_t magic;
uint8_t options[32];
} _packed;
struct pkt {
struct str raw; // Raw packet data
struct str pay; // Payload data
struct eth *eth;
struct llc *llc;
struct arp *arp;
struct ip *ip;
struct ip6 *ip6;
struct icmp *icmp;
struct tcp *tcp;
struct udp *udp;
struct dhcp *dhcp;
};
static void q_copyin(struct queue *q, const uint8_t *buf, size_t len,
size_t head) {
size_t i = 0, left = q->len - head;
for (; i < len && i < left; i++) q->buf[head + i] = buf[i];
for (; i < len; i++) q->buf[i - left] = buf[i];
}
static void q_copyout(struct queue *q, uint8_t *buf, size_t len, size_t tail) {
size_t i = 0, left = q->len - tail;
for (; i < len && i < left; i++) buf[i] = q->buf[tail + i];
for (; i < len; i++) buf[i] = q->buf[i - left];
}
static bool q_write(struct queue *q, const void *buf, size_t len) {
bool success = false;
size_t left = q->len - q->head + q->tail;
if (len + sizeof(size_t) <= left) {
q_copyin(q, (uint8_t *) &len, sizeof(len), q->head);
q_copyin(q, (uint8_t *) buf, len, (q->head + sizeof(size_t)) % q->len);
q->head = (q->head + sizeof(len) + len) % q->len;
success = true;
}
return success;
}
static size_t q_avail(struct queue *q) {
size_t n = 0;
if (q->tail != q->head) q_copyout(q, (uint8_t *) &n, sizeof(n), q->tail);
return n;
}
static size_t q_read(struct queue *q, void *buf) {
size_t n = q_avail(q);
if (n > 0) {
q_copyout(q, (uint8_t *) buf, n, (q->tail + sizeof(n)) % q->len);
q->tail = (q->tail + sizeof(n) + n) % q->len;
}
return n;
}
static struct str mkstr(void *buf, size_t len) {
struct str str = {(uint8_t *) buf, len};
return str;
}
static void mkpay(struct pkt *pkt, void *p) {
pkt->pay = mkstr(p, (size_t) (&pkt->raw.buf[pkt->raw.len] - (uint8_t *) p));
}
static uint32_t csumup(uint32_t sum, const void *buf, size_t len) {
const uint8_t *p = (const uint8_t *) buf;
for (size_t i = 0; i < len; i++) sum += i & 1 ? p[i] : (uint32_t) (p[i] << 8);
return sum;
}
static uint16_t csumfin(uint32_t sum) {
while (sum >> 16) sum = (sum & 0xffff) + (sum >> 16);
return NET16(~sum & 0xffff);
}
static uint16_t ipcsum(const void *buf, size_t len) {
uint32_t sum = csumup(0, buf, len);
return csumfin(sum);
}
// ARP cache is organised as a doubly linked list. A successful cache lookup
// moves an entry to the head of the list. New entries are added by replacing
// the last entry in the list with a new IP/MAC.
// ARP cache format: | prev | next | Entry0 | Entry1 | .... | EntryN |
// ARP entry format: | prev | next | IP (4bytes) | MAC (6bytes) |
// prev and next are 1-byte offsets in the cache, so cache size is max 256 bytes
// ARP entry size is 12 bytes
static void arp_cache_init(uint8_t *p, int n, int size) {
for (int i = 0; i < n; i++) p[2 + i * size] = (uint8_t) (2 + (i - 1) * size);
for (int i = 0; i < n; i++) p[3 + i * size] = (uint8_t) (2 + (i + 1) * size);
p[0] = p[2] = (uint8_t) (2 + (n - 1) * size);
p[1] = p[3 + (n - 1) * size] = 2;
}
static uint8_t *arp_cache_find(struct mip_if *ifp, uint32_t ip) {
uint8_t *p = ifp->arp_cache;
if (ip == 0) return NULL;
if (p[0] == 0 || p[1] == 0) arp_cache_init(p, MIP_ARP_ENTRIES, 12);
for (uint8_t i = 0, j = p[1]; i < MIP_ARP_ENTRIES; i++, j = p[j + 1]) {
if (memcmp(p + j + 2, &ip, sizeof(ip)) == 0) {
p[1] = j, p[0] = p[j]; // Found entry! Point list head to us
// MG_DEBUG(("ARP find: %#lx @ %x:%x:%x:%x:%x:%x\n", (long) ip, p[j + 6],
// p[j + 7], p[j + 8], p[j + 9], p[j + 10], p[j + 11]));
return p + j + 6; // And return MAC address
}
}
return NULL;
}
static void arp_cache_add(struct mip_if *ifp, uint32_t ip, uint8_t mac[6]) {
uint8_t *p = ifp->arp_cache;
if (ip == 0 || ip == ~0U) return; // Bad IP
if (arp_cache_find(ifp, ip) != NULL) return; // Already exists, do nothing
memcpy(p + p[0] + 2, &ip, sizeof(ip)); // Replace last entry: IP address
memcpy(p + p[0] + 6, mac, 6); // And MAC address
p[1] = p[0], p[0] = p[p[1]]; // Point list head to us
// MG_DEBUG(("ARP cache: added %#lx @ %x:%x:%x:%x:%x:%x\n", (long) ip, mac[0],
// mac[1], mac[2], mac[3], mac[4], mac[5]));
}
static void arp_ask(struct mip_if *ifp, uint32_t ip) {
struct eth *eth = (struct eth *) ifp->tx.buf;
struct arp *arp = (struct arp *) (eth + 1);
memset(eth->dst, 255, sizeof(eth->dst));
memcpy(eth->src, ifp->mac, sizeof(eth->src));
eth->type = NET16(0x806);
memset(arp, 0, sizeof(*arp));
arp->fmt = NET16(1), arp->pro = NET16(0x800), arp->hlen = 6, arp->plen = 4;
arp->op = NET16(1), arp->tpa = ip, arp->spa = ifp->ip;
memcpy(arp->sha, ifp->mac, sizeof(arp->sha));
ifp->driver->tx(eth, PDIFF(eth, arp + 1), ifp->driver->data);
}
static void onstatechange(struct mip_if *ifp) {
if (ifp->state == MIP_STATE_READY) {
char buf[40];
struct mg_addr addr = {.ip = ifp->ip};
MG_INFO(("READY, IP: %s", mg_ntoa(&addr, buf, sizeof(buf))));
arp_ask(ifp, ifp->gw);
} else if (ifp->state == MIP_STATE_UP) {
MG_ERROR(("Network up"));
} else if (ifp->state == MIP_STATE_DOWN) {
MG_ERROR(("Network down"));
}
}
static struct ip *tx_ip(struct mip_if *ifp, uint8_t proto, uint32_t ip_src,
uint32_t ip_dst, size_t plen) {
struct eth *eth = (struct eth *) ifp->tx.buf;
struct ip *ip = (struct ip *) (eth + 1);
uint8_t *mac = arp_cache_find(ifp, ip_dst); // Dst IP in ARP cache ?
if (!mac) mac = arp_cache_find(ifp, ifp->gw); // No, use gateway
if (mac) memcpy(eth->dst, mac, sizeof(eth->dst)); // Found? Use it
if (!mac) memset(eth->dst, 255, sizeof(eth->dst)); // No? Use broadcast
memcpy(eth->src, ifp->mac, sizeof(eth->src)); // TODO(cpq): ARP lookup
eth->type = NET16(0x800);
memset(ip, 0, sizeof(*ip));
ip->ver = 0x45; // Version 4, header length 5 words
ip->frag = 0x40; // Don't fragment
ip->len = NET16((uint16_t) (sizeof(*ip) + plen));
ip->ttl = 64;
ip->proto = proto;
ip->src = ip_src;
ip->dst = ip_dst;
ip->csum = ipcsum(ip, sizeof(*ip));
return ip;
}
void tx_udp(struct mip_if *ifp, uint32_t ip_src, uint16_t sport,
uint32_t ip_dst, uint16_t dport, const void *buf, size_t len) {
struct ip *ip = tx_ip(ifp, 17, ip_src, ip_dst, len + sizeof(struct udp));
struct udp *udp = (struct udp *) (ip + 1);
udp->sport = sport;
udp->dport = dport;
udp->len = NET16((uint16_t) (sizeof(*udp) + len));
udp->csum = 0;
uint32_t cs = csumup(0, udp, sizeof(*udp));
cs = csumup(cs, buf, len);
cs = csumup(cs, &ip->src, sizeof(ip->src));
cs = csumup(cs, &ip->dst, sizeof(ip->dst));
cs += ip->proto + sizeof(*udp) + len;
udp->csum = csumfin(cs);
memmove(udp + 1, buf, len);
// MG_DEBUG(("UDP LEN %d %d\n", (int) len, (int) ifp->frame_len));
ifp->driver->tx(ifp->tx.buf,
sizeof(struct eth) + sizeof(*ip) + sizeof(*udp) + len,
ifp->driver->data);
}
static void tx_dhcp(struct mip_if *ifp, uint32_t src, uint32_t dst,
uint8_t *opts, size_t optslen) {
struct dhcp dhcp = {.op = 1,
.htype = 1,
.hlen = 6,
.ciaddr = src,
.magic = NET32(0x63825363)};
memcpy(&dhcp.hwaddr, ifp->mac, sizeof(ifp->mac));
memcpy(&dhcp.xid, ifp->mac + 2, sizeof(dhcp.xid));
memcpy(&dhcp.options, opts, optslen);
tx_udp(ifp, src, NET16(68), dst, NET16(67), &dhcp, sizeof(dhcp));
}
static void tx_dhcp_request(struct mip_if *ifp, uint32_t src, uint32_t dst) {
uint8_t opts[] = {
53, 1, 3, // Type: DHCP request
55, 2, 1, 3, // GW and mask
12, 3, 'm', 'i', 'p', // Host name: "mip"
54, 4, 0, 0, 0, 0, // DHCP server ID
50, 4, 0, 0, 0, 0, // Requested IP
255 // End of options
};
memcpy(opts + 14, &dst, sizeof(dst));
memcpy(opts + 20, &src, sizeof(src));
tx_dhcp(ifp, src, dst, opts, sizeof(opts));
}
static void tx_dhcp_discover(struct mip_if *ifp) {
uint8_t opts[] = {
53, 1, 1, // Type: DHCP discover
55, 2, 1, 3, // Parameters: ip, mask
255 // End of options
};
tx_dhcp(ifp, 0, 0xffffffff, opts, sizeof(opts));
}
static void rx_arp(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("ARP op %d %#x %#x\n", NET16(arp->op), arp->spa, arp->tpa));
if (pkt->arp->op == NET16(1) && pkt->arp->tpa == ifp->ip) {
// ARP request. Make a response, then send
struct eth *eth = (struct eth *) ifp->tx.buf;
struct arp *arp = (struct arp *) (eth + 1);
memcpy(eth->dst, pkt->eth->src, sizeof(eth->dst));
memcpy(eth->src, ifp->mac, sizeof(eth->src));
eth->type = NET16(0x806);
*arp = *pkt->arp;
arp->op = NET16(2);
memcpy(arp->tha, pkt->arp->sha, sizeof(pkt->arp->tha));
memcpy(arp->sha, ifp->mac, sizeof(pkt->arp->sha));
arp->tpa = pkt->arp->spa;
arp->spa = ifp->ip;
MG_DEBUG(("ARP response: we're %#lx", (long) ifp->ip));
ifp->driver->tx(ifp->tx.buf, PDIFF(eth, arp + 1), ifp->driver->data);
} else if (pkt->arp->op == NET16(2)) {
if (memcmp(pkt->arp->tha, ifp->mac, sizeof(pkt->arp->tha)) != 0) return;
// MG_INFO(("ARP RESPONSE"));
arp_cache_add(ifp, pkt->arp->spa, pkt->arp->sha);
}
}
static void rx_icmp(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("ICMP %d\n", (int) len));
if (pkt->icmp->type == 8 && pkt->ip->dst == ifp->ip) {
struct ip *ip = tx_ip(ifp, 1, ifp->ip, pkt->ip->src,
sizeof(struct icmp) + pkt->pay.len);
struct icmp *icmp = (struct icmp *) (ip + 1);
memset(icmp, 0, sizeof(*icmp)); // Important - set csum to 0
memcpy(icmp + 1, pkt->pay.buf, pkt->pay.len);
icmp->csum = ipcsum(icmp, sizeof(*icmp) + pkt->pay.len);
ifp->driver->tx(ifp->tx.buf, PDIFF(ifp->tx.buf, icmp + 1) + pkt->pay.len,
ifp->driver->data);
}
}
static void rx_dhcp(struct mip_if *ifp, struct pkt *pkt) {
uint32_t ip = 0, gw = 0, mask = 0;
uint8_t *p = pkt->dhcp->options, *end = &pkt->raw.buf[pkt->raw.len];
if (end < (uint8_t *) (pkt->dhcp + 1)) return;
// MG_DEBUG(("DHCP %u\n", (unsigned) pkt->raw.len));
while (p < end && p[0] != 255) {
if (p[0] == 1 && p[1] == sizeof(ifp->mask)) {
memcpy(&mask, p + 2, sizeof(mask));
// MG_DEBUG(("MASK %x\n", mask));
} else if (p[0] == 3 && p[1] == sizeof(ifp->gw)) {
memcpy(&gw, p + 2, sizeof(gw));
ip = pkt->dhcp->yiaddr;
// MG_DEBUG(("IP %x GW %x\n", ip, gw));
}
p += p[1] + 2;
}
if (ip && mask && gw && ifp->ip == 0) {
// MG_DEBUG(("DHCP offer ip %#08lx mask %#08lx gw %#08lx\n",
// (long) ip, (long) mask, (long) gw));
arp_cache_add(ifp, pkt->dhcp->siaddr, ((struct eth *) pkt->raw.buf)->src);
ifp->ip = ip, ifp->gw = gw, ifp->mask = mask;
ifp->state = MIP_STATE_READY;
onstatechange(ifp);
tx_dhcp_request(ifp, ip, pkt->dhcp->siaddr);
}
}
struct mg_connection *getpeer(struct mg_mgr *mgr, struct pkt *pkt, bool lsn) {
struct mg_connection *c = NULL;
for (c = mgr->conns; c != NULL; c = c->next) {
if (c->is_udp && pkt->udp && c->loc.port == pkt->udp->dport) break;
if (!c->is_udp && pkt->tcp && c->loc.port == pkt->tcp->dport &&
lsn == c->is_listening && (lsn || c->rem.port == pkt->tcp->sport))
break;
}
return c;
}
static void rx_udp(struct mip_if *ifp, struct pkt *pkt) {
struct mg_connection *c = getpeer(ifp->mgr, pkt, true);
if (c == NULL) {
// No UDP listener on this port. Should send ICMP, but keep silent.
} else if (c != NULL) {
c->rem.port = pkt->udp->sport;
c->rem.ip = pkt->ip->src;
if (c->recv.len >= MG_MAX_RECV_BUF_SIZE) {
mg_error(c, "max_recv_buf_size reached");
} else if (c->recv.size - c->recv.len < pkt->pay.len &&
!mg_iobuf_resize(&c->recv, c->recv.len + pkt->pay.len)) {
mg_error(c, "oom");
} else {
memcpy(&c->recv.buf[c->recv.len], pkt->pay.buf, pkt->pay.len);
c->recv.len += pkt->pay.len;
struct mg_str evd = mg_str_n((char *) pkt->pay.buf, pkt->pay.len);
mg_call(c, MG_EV_READ, &evd);
}
}
}
struct tcpstate {
uint32_t seq, ack;
time_t expire;
};
static size_t tx_tcp(struct mip_if *ifp, uint32_t dst_ip, uint8_t flags,
uint16_t sport, uint16_t dport, uint32_t seq, uint32_t ack,
const void *buf, size_t len) {
struct ip *ip = tx_ip(ifp, 6, ifp->ip, dst_ip, sizeof(struct tcp) + len);
struct tcp *tcp = (struct tcp *) (ip + 1);
memset(tcp, 0, sizeof(*tcp));
memmove(tcp + 1, buf, len);
tcp->sport = sport;
tcp->dport = dport;
tcp->seq = seq;
tcp->ack = ack;
tcp->flags = flags;
tcp->win = mg_htons(8192);
tcp->off = (uint8_t) (sizeof(*tcp) / 4 << 4);
uint32_t cs = 0;
uint16_t n = (uint16_t) (sizeof(*tcp) + len);
uint8_t pseudo[] = {0, ip->proto, (uint8_t) (n >> 8), (uint8_t) (n & 255)};
cs = csumup(cs, tcp, n);
cs = csumup(cs, &ip->src, sizeof(ip->src));
cs = csumup(cs, &ip->dst, sizeof(ip->dst));
cs = csumup(cs, pseudo, sizeof(pseudo));
tcp->csum = csumfin(cs);
return ifp->driver->tx(ifp->tx.buf, PDIFF(ifp->tx.buf, tcp + 1) + len,
ifp->driver->data);
}
static size_t tx_tcp_pkt(struct mip_if *ifp, struct pkt *pkt, uint8_t flags,
uint32_t seq, const void *buf, size_t len) {
uint32_t delta = (pkt->tcp->flags & (TH_SYN | TH_FIN)) ? 1 : 0;
return tx_tcp(ifp, pkt->ip->src, flags, pkt->tcp->dport, pkt->tcp->sport, seq,
mg_htonl(mg_ntohl(pkt->tcp->seq) + delta), buf, len);
}
static struct mg_connection *accept_conn(struct mg_connection *lsn,
struct pkt *pkt) {
struct mg_connection *c = mg_alloc_conn(lsn->mgr);
struct tcpstate *s = (struct tcpstate *) (c + 1);
s->seq = mg_ntohl(pkt->tcp->ack), s->ack = mg_ntohl(pkt->tcp->seq);
c->rem.ip = pkt->ip->src;
c->rem.port = pkt->tcp->sport;
MG_DEBUG(("%lu accepted %lx:%hx", c->id, c->rem.ip, c->rem.port));
LIST_ADD_HEAD(struct mg_connection, &lsn->mgr->conns, c);
c->fd = (void *) (size_t) mg_ntohl(pkt->tcp->ack);
c->is_accepted = 1;
c->is_hexdumping = lsn->is_hexdumping;
c->pfn = lsn->pfn;
c->loc = lsn->loc;
c->pfn_data = lsn->pfn_data;
c->fn = lsn->fn;
c->fn_data = lsn->fn_data;
mg_call(c, MG_EV_OPEN, NULL);
mg_call(c, MG_EV_ACCEPT, NULL);
return c;
}
static void read_conn(struct mg_connection *c, struct pkt *pkt) {
struct tcpstate *s = (struct tcpstate *) (c + 1);
if (pkt->tcp->flags & TH_FIN) {
s->ack = mg_htonl(pkt->tcp->seq) + 1, s->seq = mg_htonl(pkt->tcp->ack);
c->is_closing = 1;
} else if (pkt->pay.len == 0) {
} else if (c->recv.size - c->recv.len < pkt->pay.len &&
!mg_iobuf_resize(&c->recv, c->recv.len + pkt->pay.len)) {
mg_error(c, "oom");
} else if (mg_ntohl(pkt->tcp->seq) != s->ack) {
mg_error(c, "oob: %x %x", mg_ntohl(pkt->tcp->seq), s->ack);
} else {
s->ack = mg_htonl(pkt->tcp->seq) + pkt->pay.len;
memcpy(&c->recv.buf[c->recv.len], pkt->pay.buf, pkt->pay.len);
c->recv.len += pkt->pay.len;
struct mg_str evd = mg_str_n((char *) pkt->pay.buf, pkt->pay.len);
mg_call(c, MG_EV_READ, &evd);
#if 0
// Send ACK immediately
tx_tcp(ifp, c->rem.ip, TH_ACK, c->loc.port, c->rem.port, mg_htonl(s->seq),
mg_htonl(s->ack), NULL, 0);
#endif
}
}
static void rx_tcp(struct mip_if *ifp, struct pkt *pkt) {
struct mg_connection *c = getpeer(ifp->mgr, pkt, false);
#if 0
MG_INFO(("%lu %hhu %d", c ? c->id : 0, pkt->tcp->flags, (int) pkt->pay.len));
#endif
if (c != NULL) {
#if 0
MG_DEBUG(("%lu %d %lx:%hx -> %lx:%hx", c->id, (int) pkt->raw.len,
pkt->ip->src, pkt->tcp->sport, pkt->ip->dst, pkt->tcp->dport));
hexdump(pkt->pay.buf, pkt->pay.len);
#endif
read_conn(c, pkt);
} else if ((c = getpeer(ifp->mgr, pkt, true)) == NULL) {
tx_tcp_pkt(ifp, pkt, TH_RST | TH_ACK, pkt->tcp->ack, NULL, 0);
} else if (pkt->tcp->flags & TH_SYN) {
// Use peer's source port as ISN, in order to recognise the handshake
uint32_t isn = mg_htonl((uint32_t) mg_ntohs(pkt->tcp->sport));
tx_tcp_pkt(ifp, pkt, TH_SYN | TH_ACK, isn, NULL, 0);
} else if (pkt->tcp->flags & TH_FIN) {
tx_tcp_pkt(ifp, pkt, TH_FIN | TH_ACK, pkt->tcp->ack, NULL, 0);
} else if (mg_htonl(pkt->tcp->ack) == mg_htons(pkt->tcp->sport) + 1U) {
accept_conn(c, pkt);
} else {
// MG_DEBUG(("dropped silently.."));
}
}
static void rx_ip(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("IP %d", (int) pkt->pay.len));
if (pkt->ip->proto == 1) {
pkt->icmp = (struct icmp *) (pkt->ip + 1);
if (pkt->pay.len < sizeof(*pkt->icmp)) return;
mkpay(pkt, pkt->icmp + 1);
rx_icmp(ifp, pkt);
} else if (pkt->ip->proto == 17) {
pkt->udp = (struct udp *) (pkt->ip + 1);
if (pkt->pay.len < sizeof(*pkt->udp)) return;
// MG_DEBUG((" UDP %u %u -> %u\n", len, NET16(udp->sport),
// NET16(udp->dport)));
mkpay(pkt, pkt->udp + 1);
if (pkt->udp->dport == NET16(68)) {
pkt->dhcp = (struct dhcp *) (pkt->udp + 1);
mkpay(pkt, pkt->dhcp + 1);
rx_dhcp(ifp, pkt);
} else {
rx_udp(ifp, pkt);
}
} else if (pkt->ip->proto == 6) {
pkt->tcp = (struct tcp *) (pkt->ip + 1);
if (pkt->pay.len < sizeof(*pkt->tcp)) return;
mkpay(pkt, pkt->tcp + 1);
uint16_t iplen = mg_ntohs(pkt->ip->len);
uint16_t off = (uint16_t) (sizeof(*pkt->ip) + ((pkt->tcp->off >> 4) * 4U));
if (iplen >= off) pkt->pay.len = (size_t) (iplen - off);
rx_tcp(ifp, pkt);
}
}
static void rx_ip6(struct mip_if *ifp, struct pkt *pkt) {
// MG_DEBUG(("IP %d\n", (int) len));
if (pkt->ip6->proto == 1 || pkt->ip6->proto == 58) {
pkt->icmp = (struct icmp *) (pkt->ip6 + 1);
if (pkt->pay.len < sizeof(*pkt->icmp)) return;
mkpay(pkt, pkt->icmp + 1);
rx_icmp(ifp, pkt);
} else if (pkt->ip->proto == 17) {
pkt->udp = (struct udp *) (pkt->ip6 + 1);
if (pkt->pay.len < sizeof(*pkt->udp)) return;
// MG_DEBUG((" UDP %u %u -> %u\n", len, NET16(udp->sport),
// NET16(udp->dport)));
mkpay(pkt, pkt->udp + 1);
}
}
static void mip_rx(struct mip_if *ifp, void *buf, size_t len) {
const uint8_t broadcast[] = {255, 255, 255, 255, 255, 255};
struct pkt pkt = {.raw = {.buf = (uint8_t *) buf, .len = len}};
pkt.eth = (struct eth *) buf;
if (pkt.raw.len < sizeof(*pkt.eth)) return; // Truncated - runt?
if (memcmp(pkt.eth->dst, ifp->mac, sizeof(pkt.eth->dst)) != 0 &&
memcmp(pkt.eth->dst, broadcast, sizeof(pkt.eth->dst)) != 0) {
// Not for us. Drop silently
} else if (pkt.eth->type == NET16(0x806)) {
pkt.arp = (struct arp *) (pkt.eth + 1);
if (sizeof(*pkt.eth) + sizeof(*pkt.arp) > pkt.raw.len) return; // Truncated
rx_arp(ifp, &pkt);
} else if (pkt.eth->type == NET16(0x86dd)) {
pkt.ip6 = (struct ip6 *) (pkt.eth + 1);
if (pkt.raw.len < sizeof(*pkt.eth) + sizeof(*pkt.ip6)) return; // Truncated
if ((pkt.ip6->ver >> 4) != 0x6) return; // Not IP
mkpay(&pkt, pkt.ip6 + 1);
rx_ip6(ifp, &pkt);
} else if (pkt.eth->type == NET16(0x800)) {
pkt.ip = (struct ip *) (pkt.eth + 1);
if (pkt.raw.len < sizeof(*pkt.eth) + sizeof(*pkt.ip)) return; // Truncated
if ((pkt.ip->ver >> 4) != 4) return; // Not IP
mkpay(&pkt, pkt.ip + 1);
rx_ip(ifp, &pkt);
} else {
MG_DEBUG((" Unknown eth type %x\n", NET16(pkt.eth->type)));
}
}
static void mip_poll(struct mip_if *ifp, uint64_t uptime_ms) {
ifp->curtime = uptime_ms;
if (ifp->ip == 0 && uptime_ms > ifp->timer) {
tx_dhcp_discover(ifp); // If IP not configured, send DHCP
ifp->timer = uptime_ms + 1000; // with some interval
} else if (ifp->use_dhcp == false && uptime_ms > ifp->timer &&
arp_cache_find(ifp, ifp->gw) == NULL) {
arp_ask(ifp, ifp->gw); // If GW's MAC address in not in ARP cache
ifp->timer = uptime_ms + 1000; // send ARP who-has request
}
// Handle physical interface up/down status
if (ifp->driver->status) {
bool up = ifp->driver->status(ifp->driver->data);
bool current = ifp->state != MIP_STATE_DOWN;
if (up != current) {
ifp->state = up == false ? MIP_STATE_DOWN
: ifp->use_dhcp ? MIP_STATE_UP
: MIP_STATE_READY;
if (!up && ifp->use_dhcp) ifp->ip = 0;
onstatechange(ifp);
}
}
// Read data from the network
for (;;) {
size_t len = ifp->queue.len > 0 ? q_read(&ifp->queue, ifp->rx.buf)
: ifp->driver->rx(ifp->rx.buf, ifp->rx.len,
ifp->driver->data);
if (len == 0) break;
mip_rx(ifp, ifp->rx.buf, len);
}
}
// This function executes in interrupt context, thus it should copy data
// somewhere fast. Note that newlib's malloc is not thread safe, thus use
// our lock-free queue with preallocated buffer to copy data and return asap
static void on_rx(void *buf, size_t len, void *userdata) {
struct mip_if *ifp = (struct mip_if *) userdata;
if (!q_write(&ifp->queue, buf, len)) MG_ERROR(("dropped %d", (int) len));
}
void mip_init(struct mg_mgr *mgr, struct mip_ipcfg *ipcfg,
struct mip_driver *driver) {
size_t maxpktsize = 1500, qlen = driver->rxcb ? 1024 * 16 : 0;
struct mip_if *ifp =
(struct mip_if *) calloc(1, sizeof(*ifp) + 2 * maxpktsize + qlen);
memcpy(ifp->mac, ipcfg->mac, sizeof(ifp->mac));
ifp->use_dhcp = ipcfg->ip == 0;
ifp->ip = ipcfg->ip, ifp->mask = ipcfg->mask, ifp->gw = ipcfg->gw;
ifp->rx.buf = (uint8_t *) (ifp + 1), ifp->rx.len = maxpktsize;
ifp->tx.buf = ifp->rx.buf + maxpktsize, ifp->tx.len = maxpktsize;
ifp->driver = driver;
ifp->mgr = mgr;
ifp->queue.buf = ifp->tx.buf + maxpktsize;
ifp->queue.len = qlen;
if (driver->init) driver->init(driver->data);
if (driver->rxcb) driver->rxcb(on_rx, ifp);
mgr->priv = ifp;
mgr->extraconnsize = sizeof(struct tcpstate);
}
void mg_connect_resolved(struct mg_connection *c) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
if (ifp->eport < MIP_ETHEMERAL_PORT) ifp->eport = MIP_ETHEMERAL_PORT;
if (c->is_udp) {
c->loc.ip = ifp->ip;
c->loc.port = mg_htons(ifp->eport++);
MG_DEBUG(("%lu %08lx.%hu->%08lx.%hu", c->id, mg_ntohl(c->loc.ip),
mg_ntohs(c->loc.port), mg_ntohl(c->rem.ip),
mg_ntohs(c->rem.port)));
mg_call(c, MG_EV_RESOLVE, NULL);
mg_call(c, MG_EV_CONNECT, NULL);
} else {
mg_error(c, "Not implemented");
}
c->is_resolving = 0;
}
bool mg_open_listener(struct mg_connection *c, const char *url) {
c->loc.port = mg_htons(mg_url_port(url));
return true;
}
static void write_conn(struct mg_connection *c) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
struct tcpstate *s = (struct tcpstate *) (c + 1);
size_t sent, n = c->send.len, hdrlen = 14 + 24 /*max IP*/ + 60 /*max TCP*/;
if (n + hdrlen > ifp->tx.len) n = ifp->tx.len - hdrlen;
sent = tx_tcp(ifp, c->rem.ip, TH_PUSH | TH_ACK, c->loc.port, c->rem.port,
mg_htonl(s->seq), mg_htonl(s->ack), c->send.buf, n);
if (sent > 0) {
mg_iobuf_del(&c->send, 0, n);
s->seq += n;
mg_call(c, MG_EV_WRITE, &n);
}
}
static void fin_conn(struct mg_connection *c) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
struct tcpstate *s = (struct tcpstate *) (c + 1);
tx_tcp(ifp, c->rem.ip, TH_FIN | TH_ACK, c->loc.port, c->rem.port,
mg_htonl(s->seq), mg_htonl(s->ack), NULL, 0);
}
void mg_mgr_poll(struct mg_mgr *mgr, int ms) {
struct mg_connection *c, *tmp;
uint64_t now = mg_millis();
mip_poll((struct mip_if *) mgr->priv, now);
mg_timer_poll(&mgr->timers, now);
for (c = mgr->conns; c != NULL; c = tmp) {
tmp = c->next;
if (c->send.len > 0) write_conn(c);
if (c->is_draining && c->send.len == 0) c->is_closing = 1;
if (c->is_closing) {
if (c->is_udp == false && c->is_listening == false) fin_conn(c);
mg_close_conn(c);
}
}
(void) ms;
}
bool mg_send(struct mg_connection *c, const void *buf, size_t len) {
struct mip_if *ifp = (struct mip_if *) c->mgr->priv;
bool res = false;
if (ifp->ip == 0) {
mg_error(c, "net down");
} else if (c->is_udp) {
tx_udp(ifp, ifp->ip, c->loc.port, c->rem.ip, c->rem.port, buf, len);
res = true;
} else {
// tx_tdp(ifp, ifp->ip, c->loc.port, c->rem.ip, c->rem.port, buf, len);
return mg_iobuf_add(&c->send, c->send.len, buf, len, MG_IO_SIZE);
}
return res;
}
int mg_mkpipe(struct mg_mgr *mgr, mg_event_handler_t fn, void *fn_data) {
(void) mgr, (void) fn, (void) fn_data;
return -1;
}
#endif // MG_ENABLE_MIP

21
src/mip.h Normal file
View File

@ -0,0 +1,21 @@
#pragma once
#include "arch.h"
#include "net.h"
struct mip_driver {
void *data; // Driver-specific data
void (*init)(void *data); // Initialise driver
size_t (*tx)(const void *, size_t, void *data); // Transmit frame
size_t (*rx)(void *buf, size_t len, void *data); // Receive frame (polling)
bool (*status)(void *data); // Up/down status
// Set receive callback for interrupt-driven drivers
void (*rxcb)(void (*fn)(void *buf, size_t len, void *rxdata), void *rxdata);
};
struct mip_ipcfg {
uint8_t mac[6]; // MAC address. Must not be 0
uint32_t ip, mask, gw; // IP, netmask, GW. If IP is 0, DHCP is used
};
void mip_init(struct mg_mgr *, struct mip_ipcfg *, struct mip_driver *);