Skip to content

Commit

Permalink
ADD: add RX functionality
Browse files Browse the repository at this point in the history
  • Loading branch information
@T-K-233
T-K-233 committed Jun 27, 2024
1 parent 3707fa0 commit f4584bf
Show file tree
Hide file tree
Showing 2 changed files with 203 additions and 84 deletions.
176 changes: 141 additions & 35 deletions src/sim_udp.cc
View file
Original file line number Diff line number Diff line change
@@ -1,48 +1,134 @@
#include "sim_udp.h"

void sim_udp_t::udp_receive() {
// UDPSocket *udp = (UDPSocket *)udp_ptr;

printf("<SimUDP> [INFO]: UDP Rx thread started\n");

socklen_t len = sizeof(this->udp.rx_addr);

printf("here\n");

// create performance counter
// struct timespec start, end;

// clock_gettime(CLOCK_MONOTONIC, &start);

while (1) {
// int n = recvfrom(udp->sockfd, (void *)rx_buffer, sizeof(float) * N_OBSERVATIONS, MSG_WAITALL, (struct sockaddr *)&udp->robot_addr, &len);

// if (n == sizeof(float) * N_OBSERVATIONS) {
// for (int i = 0; i < N_OBSERVATIONS; i++) {
// udp->obs[i] = rx_buffer[i];
// }
// }
// else {
// // sleep for 1ms
// usleep(1000);
// }


if (this->rx_enabled) {
socklen_t len = sizeof(this->udp.rx_addr);
int n = recvfrom(this->udp.sockfd, (void *)this->rx_fifo, 512, MSG_WAITALL, (struct sockaddr *)&this->udp.rx_addr, &len);

if (n) {
printf("<SimUDP> [INFO]: UDP Rx from (%s, %d) with data size: %d\n",
inet_ntoa(this->udp.rx_addr.sin_addr),
ntohs(this->udp.rx_addr.sin_port),
n
);
}
}
}
// // perfromance
// clock_gettime(CLOCK_MONOTONIC, &end);

void sim_udp_t::write_udp_tx() {
printf("<SimUDP> [INFO]: Writing UDP TX with DMA pointer: 0x%lx, DMA size: %d\n", tx_fifo, reg_tx_size);
printf("addr: %x, port: %x\n", reg_addr_ip, reg_addr_tx_port);
sendto(udp->sockfd, (uint8_t *)tx_fifo, reg_tx_size, 0, (const struct sockaddr *)&udp->host_addr, sizeof(udp->host_addr));
}

void sim_udp_t::create_udp_tx() {
udp = (UDPRx *)malloc(sizeof(UDPRx));
// long seconds = end.tv_sec - start.tv_sec;
// long ns = end.tv_nsec - start.tv_nsec;

// // Correct for rollover
// if (start.tv_nsec > end.tv_nsec) {
// --seconds;
// ns += 1000000000;
// }

memset(&udp->client_addr, 0, sizeof(udp->client_addr));
memset(&udp->host_addr, 0, sizeof(udp->host_addr));
// clock_gettime(CLOCK_MONOTONIC, &start);

// host ip
udp->host_addr.sin_family = AF_INET;
udp->host_addr.sin_addr.s_addr = htonl(reg_addr_ip);
udp->host_addr.sin_port = htons(reg_addr_tx_port);
// double freq = 1.0 / (seconds + ns / 1000000000.0);

// // printf("Frequency: %f\n", freq);
// }
}

void sim_udp_t::udp_create_socket() {
// Create socket file descriptor
if ((udp->sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
if ((this->udp.sockfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
printf("<SimUDP> [ERROR]: socket creation failed\n");
}

// if (bind(udp->sockfd, (const struct sockaddr *)&udp->host_addr, sizeof(udp->host_addr)) < 0) {
// printf("<SimUDP> [ERROR]: bind failed\n");
printf("<SimUDP> [INFO]: socket created\n");
}

void sim_udp_t::udp_enable_rx() {
if (bind(udp.sockfd, (const struct sockaddr *)&udp.rx_addr, sizeof(udp.rx_addr)) < 0) {
printf("<SimUDP> [ERROR]: bind failed\n");
}

std::thread t1(&sim_udp_t::udp_receive, this);
t1.detach();

// // Create a thread running the receive() function
// if(pthread_create(&this->udp.thread_id, NULL, this->udp_receive, (void *)this->udp) != 0) {
// perror("[ERROR]: Failed to create receive thread");
// }

// Create a thread running the receive() function
this->rx_enabled = 1;


printf("<SimUDP> [INFO]: Rx thread created, thread ID: %ld\n", (long)this->udp.thread_id);
}

void sim_udp_t::udp_enable_tx() {

printf("<SimUDP> [INFO]: Tx thread created, thread ID: %ld\n", (long)udp->thread_id);

this->tx_enabled = 1;
printf("<SimUDP> [INFO]: Tx thread created, thread ID: %ld\n", (long)this->udp.thread_id);
}

void sim_udp_t::udp_write() {

if (!this->tx_enabled) {
return;
}

printf("<SimUDP> [INFO]: UDP Tx to (%s, %d) with data size: %d\n",
inet_ntoa(this->udp.tx_addr.sin_addr),
ntohs(this->udp.tx_addr.sin_port),
this->reg_txsize
);

sendto(
this->udp.sockfd,
(uint8_t *)this->tx_fifo, this->reg_txsize, 0,
(const struct sockaddr *)&this->udp.tx_addr, sizeof(this->udp.tx_addr)
);
}

bool sim_udp_t::load(reg_t addr, size_t len, uint8_t* bytes) {
printf("LOAD -- ADDR=0x%lx LEN=%lu\n", addr, len);
if (addr >= 0x1000 || len > 4) return false;
uint32_t r = 0;
switch (addr) {
case UDP_IP: r = reg_addr_ip; break;
case UDP_RXPORT: r = reg_addr_rx_port; break;
case UDP_TXPORT: r = reg_addr_tx_port; break;
case UDP_RXSIZE: r = reg_rx_size; break;
case UDP_RXIP: r = this->udp.rx_addr.sin_addr.s_addr; break;
case UDP_TXIP: r = this->udp.tx_addr.sin_addr.s_addr; break;
case UDP_RXPORT: r = this->udp.rx_addr.sin_port; break;
case UDP_TXPORT: r = this->udp.tx_addr.sin_port; break;
case UDP_CTRL: r = this->reg_ctrl; break;
case UDP_STATUS: r = this->reg_status; break;
case UDP_RXSIZE: r = this->reg_rxsize; break;
case UDP_TXSIZE: r = this->reg_txsize; break;
default: printf("LOAD -- ADDR=0x%lx LEN=%lu\n", addr, len); abort();
}
memcpy(bytes, &r, len);
Expand All @@ -51,7 +137,7 @@ bool sim_udp_t::load(reg_t addr, size_t len, uint8_t* bytes) {


bool sim_udp_t::store(reg_t addr, size_t len, const uint8_t* bytes) {
printf("STORE -- ADDR=0x%lx LEN=%lu\n", addr, len);
printf("STORE -- ADDR=0x%lx LEN=%lu DATA=%lx\n", addr, len, *(uint32_t *)bytes);

if (addr >= 0x1000 || len > 4) return false;

Expand All @@ -60,24 +146,44 @@ bool sim_udp_t::store(reg_t addr, size_t len, const uint8_t* bytes) {
return true;
}
switch (addr) {
case UDP_IP: reg_addr_ip = *((uint32_t *)bytes); return true;
case UDP_RXPORT: reg_addr_rx_port = *((uint16_t *)bytes); return true;
case UDP_TXPORT: reg_addr_tx_port = *((uint16_t *)bytes); return true;
case UDP_TXCTRL:
if (bytes[0] == 1) write_udp_tx();
if (bytes[0] == 2) create_udp_tx();
case UDP_RXIP:
this->udp.rx_addr.sin_addr.s_addr = *((uint32_t *)bytes);
return true;
case UDP_TXIP:
this->udp.tx_addr.sin_addr.s_addr = *((uint32_t *)bytes);
return true;
case UDP_RXPORT:
this->udp.rx_addr.sin_port = *((uint16_t *)bytes);
return true;
case UDP_TXSIZE: reg_tx_size = *((uint32_t *)bytes); return true;
case UDP_TXPORT:
this->udp.tx_addr.sin_port = *((uint16_t *)bytes);
return true;
case UDP_CTRL:
if (bytes[0] & (1 << 0)) {
this->udp_create_socket();
}
if (bytes[0] & (1 << 1)) {
this->udp_enable_rx();
}
if (bytes[0] & (1 << 2)) {
this->udp_enable_tx();
}
if (bytes[0] & (1 << 3)) {
this->udp_write();
}
return true;
case UDP_TXSIZE: this->reg_txsize = *((uint32_t *)bytes); return true;
default: printf("STORE -- ADDR=0x%lx LEN=%lu\n", addr, len); abort();
}
}

void sim_udp_t::tick(reg_t UNUSED rtc_ticks) {
if (rx_fifo.size() >= 1) return;
int rc = canonical_terminal_t::read();
if (rc < 0) return;
rx_fifo.push((uint8_t)rc);
update_interrupts();
// if (rx_fifo.size() >= 1) return;
// int rc = canonical_terminal_t::read();
// if (rc < 0) return;
// rx_fifo.push((uint8_t)rc);
// update_interrupts();

}

int SimUDP_parseFDT(const void *fdt, reg_t *address,
Expand Down
111 changes: 62 additions & 49 deletions src/sim_udp.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,83 +14,96 @@
#include <netinet/in.h>
#include <unistd.h>
#include <arpa/inet.h>
#include <pthread.h>
#include <thread>



#define UDP_IP 0x00
#define UDP_RXPORT 0x04
#define UDP_TXPORT 0x08
#define UDP_RXCTRL 0x0C
#define UDP_TXCTRL 0x10
#define UDP_RXSTATUS 0x14
#define UDP_TXSTATUS 0x18
#define UDP_RXSIZE 0x1C
#define UDP_TXSIZE 0x20
#define UDP_RXIP 0x00
#define UDP_TXIP 0x04
#define UDP_RXPORT 0x08
#define UDP_TXPORT 0x0C
#define UDP_CTRL 0x10
#define UDP_STATUS 0x14
#define UDP_RXSIZE 0x18
#define UDP_TXSIZE 0x1C
#define UDP_RXFIFO 0x30
#define UDP_TXFIFO 0x130


typedef struct {
int sockfd;
pthread_t thread_id;
struct sockaddr_in client_addr;
struct sockaddr_in host_addr;
} UDPRx;
struct sockaddr_in tx_addr;
struct sockaddr_in rx_addr;
} UDPSocket;

class sim_udp_t : public abstract_device_t {
public:
sim_udp_t(abstract_interrupt_controller_t *intctrl, reg_t int_id) :
ie(0), ip(0), txctrl(0), rxctrl(0), div(0), interrupt_id(int_id), intctrl(intctrl) {
sim_udp_t(abstract_interrupt_controller_t *intctrl, reg_t int_id) {
this->interrupt_id = int_id;
this->intctrl = intctrl;

this->udp.rx_addr.sin_family = AF_INET;
this->udp.tx_addr.sin_family = AF_INET;

this->reg_ctrl = 0;
this->reg_status = 0;
this->reg_rxsize = 0;
this->reg_txsize = 0;

this->rx_enabled = 0;
this->tx_enabled = 0;

}

bool load(reg_t addr, size_t len, uint8_t* bytes) override;
bool store(reg_t addr, size_t len, const uint8_t* bytes) override;
void tick(reg_t UNUSED rtc_ticks) override;

private:
std::queue<uint8_t> rx_fifo;
uint32_t reg_ctrl;
uint32_t reg_status;
uint32_t reg_rxsize;
uint32_t reg_txsize;

// std::queue<uint8_t> rx_fifo;
uint8_t rx_fifo[256];
uint8_t tx_fifo[256];

uint32_t ie;
uint32_t ip;
uint32_t txctrl;
uint32_t rxctrl;

uint8_t tx_fifo[256];

uint32_t reg_rx_size;
uint32_t reg_tx_size;

uint32_t div;
reg_t interrupt_id;
abstract_interrupt_controller_t *intctrl;

uint32_t reg_addr_ip;
uint32_t reg_addr_rx_port;
uint32_t reg_addr_tx_port;

UDPRx *udp;

void write_udp_tx();
void create_udp_tx();

uint32_t read_rxfifo() {
if (!rx_fifo.size()) return 0x80000000;
uint8_t r = rx_fifo.front();
rx_fifo.pop();
update_interrupts();
return r;
}

void update_interrupts() {
int cond = 0;
if ((ie) ||
((ie) && rx_fifo.size())) {
cond = 1;
}
intctrl->set_interrupt_level(interrupt_id, (cond) ? 1 : 0);
}
UDPSocket udp;
// UDPSocket *udp_rx;

uint8_t rx_enabled;
uint8_t tx_enabled;

void udp_create_socket();
void udp_enable_rx();
void udp_enable_tx();
void udp_write();
void udp_receive();

// uint32_t read_rxfifo() {
// if (!rx_fifo.size()) return 0x80000000;
// uint8_t r = rx_fifo.front();
// rx_fifo.pop();
// update_interrupts();
// return r;
// }

// void update_interrupts() {
// int cond = 0;
// if ((ie) ||
// ((ie) && rx_fifo.size())) {
// cond = 1;
// }
// intctrl->set_interrupt_level(interrupt_id, (cond) ? 1 : 0);
// }
};

#endif // _SIM_UDP_H

0 comments on commit f4584bf

Please sign in to comment.