kamkow1/my-os-project2
1
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Porting PicoTCP WIP

Browse Source
This commit is contained in:
Kamil Kowalczyk
2025-10-29 14:29:06 +01:00
parent 6722f42e68
commit 815c2239fe
464 changed files with 235009 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files

486
kernel/picotcp/modules/pico_dev_radiotest.c Normal file
View File

@ -0,0 +1,486 @@
/*********************************************************************
PicoTCP. Copyright (c) 2012-2017 Altran Intelligent Systems. Some rights reserved.
See LICENSE and COPYING for usage.
Authors: Daniele Lacamera, Jelle De Vleeschouwer
*********************************************************************/
/*******************************************************************************
* PicoTCP
******************************************************************************/
#include "pico_dev_radiotest.h"
#include "pico_6lowpan_ll.h"
#include "pico_addressing.h"
#include "pico_dev_tap.h"
#include "pico_802154.h"
#include "pico_device.h"
#include "pico_config.h"
#include "pico_stack.h"
/*******************************************************************************
* System sockets
******************************************************************************/
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <sys/poll.h>
#include <signal.h>
#include <errno.h>
#define LISTENING_PORT 7777
#define MESSAGE_MTU 150
#ifdef RADIO_PCAP
#include <pcap.h>
#endif
#ifdef DEBUG_RADIOTEST
#define RADIO_DBG dbg
#else
#define RADIO_DBG(...) do { } while (0)
#endif
/*******************************************************************************
* Constants
******************************************************************************/
/* Uncomment next line to enable Random packet loss (specify percentage) */
//#define PACKET_LOSS 3
#define RFDEV_PANID 0xABCD
#define MC_ADDR_BE 0x010101EBU
#define LO_ADDR 0x0100007FU
#define LOOP_MTU 127
/*******************************************************************************
* Type definitions
******************************************************************************/
struct radiotest_radio {
struct pico_dev_6lowpan dev;
struct pico_6lowpan_info addr;
int sock0;
int sock1;
#ifdef RADIO_PCAP
pcap_t *pcap;
pcap_dumper_t *pcapd;
#endif
};
struct radiotest_frame
{
uint8_t *buf;
int len;
uint32_t id;
union pico_ll_addr src;
union pico_ll_addr dst;
};
/*******************************************************************************
* Global variables
******************************************************************************/
static int connection = 0;
static uint32_t tx_id = 0;
static uint32_t rx_id = 0;
/*******************************************************************************
* pcap
******************************************************************************/
#ifdef RADIO_PCAP
static void radiotest_pcap_open(struct radiotest_radio *dev, char *dump)
{
char path[100];
/* Open offline packet capture */
#ifdef PICO_SUPPORT_802154
dev->pcap = pcap_open_dead(DLT_IEEE802_15_4, 65535);
#elif defined (PICO_SUPPORT_802154_NO_MAC)
dev->pcap = pcap_open_dead(DLT_RAW, 65535);
#endif
if (!dev->pcap) {
perror("LibPCAP");
exit (1);
}
/* Construct file path */
snprintf(path, 100, dump, dev->addr);
/* Open dump */
dev->pcapd = pcap_dump_open(dev->pcap, path);
if (dev->pcapd)
dbg("PCAP Enabled\n");
else
dbg("PCAP Disabled\n");
}
static void radiotest_pcap_write(struct radiotest_radio *dev, uint8_t *buf, int len)
{
struct pcap_pkthdr ph;
if (!dev || !dev->pcapd)
return;
ph.caplen = (uint32_t)len;
ph.len = (uint32_t)len;
gettimeofday(&ph.ts, NULL);
pcap_dump((u_char *)dev->pcapd, &ph, buf);
pcap_dump_flush(dev->pcapd);
}
#else
static void radiotest_pcap_open(struct radiotest_radio *dev, char *dump)
{
(void)dev;
(void)dump;
}
static void radiotest_pcap_write(struct radiotest_radio *dev, uint8_t *buf, int len)
{
(void)dev;
(void)buf;
(void)len;
}
#endif
static int radiotest_cmp(void *a, void *b)
{
struct radiotest_frame *fa = (struct radiotest_frame *)a;
struct radiotest_frame *fb = (struct radiotest_frame *)b;
return (int)(fa->id - fb->id);
}
PICO_TREE_DECLARE(LoopFrames, radiotest_cmp);
static uint8_t *radiotest_nxt_rx(int *len, union pico_ll_addr *src, union pico_ll_addr *dst)
{
struct radiotest_frame test, *found = NULL;
uint8_t *ret = NULL;
test.id = rx_id++;
found = pico_tree_findKey(&LoopFrames, &test);
if (found) {
ret = found->buf;
*len = found->len;
*src = found->src;
*dst = found->dst;
pico_tree_delete(&LoopFrames, found);
PICO_FREE(found);
} else {
rx_id--;
}
return ret;
}
static void radiotest_nxt_tx(uint8_t *buf, int len, union pico_ll_addr src, union pico_ll_addr dst)
{
struct radiotest_frame *new = PICO_ZALLOC(sizeof(struct radiotest_frame));
if (new) {
new->buf = PICO_ZALLOC((uint16_t)len);
if (new->buf) {
memcpy(new->buf, buf, (size_t)len);
new->len = len;
new->id = tx_id++;
new->src = src;
new->dst = dst;
if (pico_tree_insert(&LoopFrames, new)) {
PICO_FREE(new);
tx_id--;
}
} else {
PICO_FREE(new);
}
}
}
static int pico_loop_send(struct pico_device *dev, void *buf, int len, union pico_ll_addr src, union pico_ll_addr dst)
{
IGNORE_PARAMETER(dev);
if (len > LOOP_MTU)
return 0;
RADIO_DBG("Looping back frame of %d bytes.\n", len);
radiotest_nxt_tx(buf, len, src, dst);
return len;
}
static int pico_loop_poll(struct pico_device *dev, int loop_score)
{
union pico_ll_addr src, dst;
uint8_t *buf = NULL;
int len = 0;
if (loop_score <= 0)
return 0;
buf = radiotest_nxt_rx(&len, &src, &dst);
if (buf) {
RADIO_DBG("Receiving frame of %d bytes.\n", len);
pico_6lowpan_stack_recv(dev, buf, (uint32_t)len, &src, &dst);
PICO_FREE(buf);
loop_score--;
}
return loop_score;
}
/* Generates a simple extended address */
static void radiotest_gen_ex(struct pico_6lowpan_short addr_short, uint8_t *buf)
{
uint16_t sh = addr_short.addr;
buf[0] = 0x00;
buf[1] = 0x00;
buf[2] = 0x00;
buf[3] = 0xaa;
buf[4] = 0xab;
buf[5] = 0x00;
buf[6] = (uint8_t)((uint8_t)(short_be(sh) & 0xFF00) >> 8u);
buf[7] = (uint8_t)(short_be(sh) & 0xFFu);
}
/**
* Simulated CRC16-CITT Kermit generation
*
* @param buf uint8_t *, buffer to generate FCS for.
* @param len uint8_t, len of the buffer
*
* @return CITT Kermit CRC16 of the buffer
*/
static uint16_t calculate_crc16(uint8_t *buf, uint8_t len)
{
uint16_t crc = 0x0000;
uint16_t q = 0, i = 0;
uint8_t c = 0;
for (i = 0; i < len; i++) {
c = buf[i];
q = (crc ^ c) & 0x0F;
crc = (uint16_t)((uint16_t)(crc >> 4) ^ (q * 0x1081));
q = (crc ^ (c >> 4)) & 0xF;
crc = (uint16_t)((uint16_t)(crc >> 4) ^ (q * 0x1081));
}
return crc;
}
/* Poll-function for the pico_device-structure */
static int radiotest_poll(struct pico_device *dev, int loop_score)
{
struct radiotest_radio *radio = (struct radiotest_radio *)dev;
union pico_ll_addr src = {0}, dst = {0};
int pollret, ret_len;
struct pollfd p;
uint8_t buf[128];
uint8_t phy = 0;
if (loop_score <= 0)
return 0;
if (!dev)
return loop_score;
p.fd = connection;
p.events = POLLIN | POLLHUP;
/* Poll for data from radio management */
errno = 0;
pollret = poll(&p, (nfds_t)1, 1);
if (errno == EINTR || pollret == 0)
return loop_score;
if (pollret < 0) {
fprintf(stderr, "Socket error %s!\n", strerror(errno));
exit(5);
}
if (p.revents & POLLIN) {
ret_len = (int)recv(connection, &phy, (size_t)1, 0);
if (ret_len != 1) return loop_score;
ret_len = (int)recv(connection, buf, (size_t)phy, 0);
if (ret_len != (int)phy)
return loop_score;
else if (!ret_len) {
RADIO_DBG("Radio manager detached from network\n");
exit(1);
}
}
if (ret_len < 2) { /* Not valid */
return loop_score;
}
#ifdef P_LOSS
long n = lrand48();
n = n % 100;
if (n < P_LOSS) {
RADIO_DBG("Packet got lost!\n");
return loop_score;
}
#endif
/* ADDRESS FILTER */
if (buf[ret_len - 1] != 0xFF && buf[ret_len - 1] != (uint8_t)short_be(radio->addr.addr_short.addr)) {
RADIO_DBG("Packet is not for me!\n");
return loop_score;
}
/* Get src and destination address */
dst.pan.addr._ext = radio->addr.addr_ext;
src.pan.addr.data[3] = 0xAA;
src.pan.addr.data[4] = 0xAB;
src.pan.addr.data[7] = buf[ret_len - 1];
src.pan.mode = AM_6LOWPAN_EXT;
ret_len -= 2;
/* Write the received frame to the pcap-dump */
radiotest_pcap_write(radio, buf, ret_len);
/* Hand the frame over to pico */
pico_6lowpan_stack_recv(dev, buf, (uint32_t)(ret_len - 2), &src, &dst);
loop_score--;
return loop_score;
}
#define RADIO_OVERHEAD 4
/* Send-function for the pico_device-structure */
static int radiotest_send(struct pico_device *dev, void *_buf, int len, union pico_ll_addr src, union pico_ll_addr dst)
{
struct radiotest_radio *radio = (struct radiotest_radio *)dev;
uint8_t *buf = PICO_ZALLOC((size_t)(len + RADIO_OVERHEAD));
uint8_t phy = 0, did = 0;
uint16_t crc = 0;
int ret = 0, dlen = 0;
IGNORE_PARAMETER(src);
if (!buf)
return -1;
/* Try to get node-ID from address */
if (dev && pico_6lowpan_lls[dev->mode].addr_len) {
dlen = pico_6lowpan_lls[dev->mode].addr_len(&dst);
if (dlen < 0)
return -1;
did = dst.pan.addr.data[dlen - 1];
}
/* Store the addresses in buffer for management */
memcpy(buf, _buf, (size_t)len);
len = (uint16_t)(len + (uint16_t)RADIO_OVERHEAD); // CRC + ID
buf[len - 2] = (uint8_t)short_be(radio->addr.addr_short.addr);
buf[len - 1] = did;
/* Generate FCS, keep pcap happy ... */
crc = calculate_crc16(_buf, (uint8_t)(len - RADIO_OVERHEAD));
memcpy(buf + len - RADIO_OVERHEAD, (void *)&crc, 2);
/* Send frame to radio management */
phy = (uint8_t)(len);
ret = (int)sendto(connection, &phy, 1, 0, NULL, 0);
if (ret != 1)
return -1;
ret = (int)sendto(connection, buf, (size_t)(len), 0, NULL, 0);
RADIO_DBG("Radio '%u' transmitted a frame of %d bytes.\n", buf[len - 2], ret);
/* Write the sent frame to the pcap-dump */
radiotest_pcap_write(radio, buf, len - 2);
PICO_FREE(buf);
return ret;
}
static int radiotest_hello(int s, uint8_t id, uint8_t area0, uint8_t area1)
{
uint8_t buf[3] = { id, area0, area1 };
if (sendto(s, buf, (size_t)3, 0, NULL, 0) != 3) {
RADIO_DBG("Radio '%u' failed to send hello message\n", id);
return -1;
}
RADIO_DBG("Radio '%u' attached to network\n", id);
return s;
}
static int radiotest_connect(uint8_t id, uint8_t area0, uint8_t area1)
{
struct sockaddr_in addr;
int s = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
int ret = 0;
memset(&addr, 0, sizeof(struct sockaddr_in));
addr.sin_family = AF_INET;
addr.sin_port = htons(LISTENING_PORT);
inet_pton(AF_INET, "127.0.0.1", &addr.sin_addr);
ret = connect(s, (struct sockaddr *)&addr, sizeof(struct sockaddr_in));
if (ret) {
RADIO_DBG("Radio '%u' could not attach to network\n", id);
return ret;
}
return radiotest_hello(s, id, area0, area1);
}
static void
pico_radiotest_quit(int signum)
{
IGNORE_PARAMETER(signum);
dbg("Quitting radiotest\n");
exit(0);
}
/* Creates a radiotest-device */
struct pico_device *pico_radiotest_create(uint8_t addr, uint8_t area0, uint8_t area1, int loop, char *dump)
{
struct radiotest_radio *radio = PICO_ZALLOC(sizeof(struct radiotest_radio));
struct pico_dev_6lowpan *lp = (struct pico_dev_6lowpan *)radio;
if (!radio)
return NULL;
if (!addr || (addr && !area0)) {
RADIO_DBG("Usage (node): -6 [1-255],[1-255],[0-255] ...\n");
}
signal(SIGQUIT, pico_radiotest_quit);
radio->addr.pan_id.addr = short_be(RFDEV_PANID);
radio->addr.addr_short.addr = short_be((uint16_t)addr);
radiotest_gen_ex(radio->addr.addr_short, radio->addr.addr_ext.addr);
RADIO_DBG("Radiotest short address: 0x%04X\n", short_be(radio->addr.addr_short.addr));
RADIO_DBG("Radiotest ext address: %02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X\n",
radio->addr.addr_ext.addr[0],radio->addr.addr_ext.addr[1],
radio->addr.addr_ext.addr[2],radio->addr.addr_ext.addr[3],
radio->addr.addr_ext.addr[4],radio->addr.addr_ext.addr[5],
radio->addr.addr_ext.addr[6],radio->addr.addr_ext.addr[7]);
if (!loop) {
if ((connection = radiotest_connect(addr, area0, area1)) <= 0) {
return NULL;
}
if (pico_dev_6lowpan_init(lp, "radio", (uint8_t *)&radio->addr, LL_MODE_IEEE802154, MTU_802154_MAC, 0, radiotest_send, radiotest_poll)) {
RADIO_DBG("pico_device_init failed.\n");
pico_device_destroy((struct pico_device *)lp);
return NULL;
}
} else {
if (pico_dev_6lowpan_init(lp, "radio", (uint8_t *)&radio->addr, LL_MODE_IEEE802154, MTU_802154_MAC, 0, pico_loop_send, pico_loop_poll)) {
RADIO_DBG("pico_device_init failed.\n");
pico_device_destroy((struct pico_device *)lp);
return NULL;
}
}
if (dump) {
dbg("Dump: %s\n", dump);
radiotest_pcap_open(radio, dump);
}
return (struct pico_device *)lp;
}