Merge pull request #2623 from cesanta/r2d2

Add support for RA6M4
This commit is contained in:
Sergey Lyubka 2024-02-23 00:45:47 +00:00 committed by GitHub
commit 1743e175cd
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
11 changed files with 767 additions and 87 deletions

View File

@ -4,14 +4,11 @@ CFLAGS += -g3 -Os -ffunction-sections -fdata-sections
CFLAGS += -I. -Icmsis_core/CMSIS/Core/Include
CFLAGS += -Icmsis_mcu/ra/fsp/src/bsp/cmsis/Device/RENESAS/Include
CFLAGS += -Icmsis_mcu/ra/fsp/src/bsp/mcu/all
#CFLAGS += -Icmsis_mcu/ra/fsp/inc
#CFLAGS += -Icmsis_mcu/ra/fsp/inc/api
CFLAGS += -mcpu=cortex-m33 -mthumb -mfpu=fpv5-sp-d16 -mfloat-abi=hard $(CFLAGS_EXTRA)
LDFLAGS ?= -Tlink.ld -nostdlib -nostartfiles --specs nano.specs -lc -lgcc -Wl,--gc-sections -Wl,-Map=$@.map
SOURCES = main.c hal.c mongoose.c net.c packed_fs.c
#SOURCES += cmsis_mcu/Files/startup_RA.c
#SOURCES += cmsis_mcu/Files/system_RA.c
# CMSIS startup and system files are deeply tied to the BSP, and we don't use it. See hal.c
# Example specific build options. See README.md
CFLAGS += -DHTTP_URL=\"http://0.0.0.0/\" -DHTTPS_URL=\"https://0.0.0.0/\"
@ -42,11 +39,11 @@ cmsis_mcu:
# Automated remote test. Requires env variable VCON_API_KEY set. See https://vcon.io/automated-firmware-tests/
DEVICE_URL ?= https://dash.vcon.io/api/v3/devices/13
DEVICE_URL ?= https://dash.vcon.io/api/v3/devices/14
update: firmware.bin
curl --fail-with-body -su :$(VCON_API_KEY) $(DEVICE_URL)/ota --data-binary @$<
update: CFLAGS += -DUART_DEBUG=LPUART3
update: CFLAGS += -DUART_DEBUG=R_SCI7
test: update
curl --fail-with-body -su :$(VCON_API_KEY) $(DEVICE_URL)/tx?t=5 | tee /tmp/output.txt

View File

@ -13,11 +13,12 @@ static volatile uint64_t s_ticks; // Milliseconds since boot
extern void _estack(void); // Defined in link.ld
void Reset_Handler(void);
void SysTick_Handler(void);
extern void EDMAC_IRQHandler(void);
// 16 ARM and 200 peripheral handlers
__attribute__((section(".vectors"))) void (*const tab[16 + 10])(void) = {
_estack, Reset_Handler, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, SysTick_Handler};
_estack, Reset_Handler, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
SysTick_Handler, EDMAC_IRQHandler};
void Reset_Handler(void) {
extern long _sbss[], _ebss[], _sdata[], _edata[], _sidata[];
@ -33,8 +34,11 @@ void Reset_Handler(void) {
}
void SystemInit(void) { // Called automatically by startup code
clock_init(); // Set system clock to SYS_FREQUENCY
rng_init(); // Initialise random number generator
SCB->CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); // Enable FPU
__DSB();
__ISB();
clock_init(); // Set system clock to SYS_FREQUENCY
rng_init(); // Initialise random number generator
SysTick_Config(SystemCoreClock / 1000); // Sys tick every 1ms
}
@ -42,12 +46,14 @@ void SysTick_Handler(void) { // SyStick IRQ handler, triggered every 1ms
s_ticks++;
}
#if 0
void mg_random(void *buf, size_t len) { // Use on-board RNG
for (size_t n = 0; n < len; n += sizeof(uint32_t)) {
uint32_t r = rng_read();
memcpy((char *) buf + n, &r, n + sizeof(r) > len ? len - n : sizeof(r));
}
}
#endif
uint64_t mg_millis(void) { // Let Mongoose use our uptime function
return s_ticks; // Return number of milliseconds since boot

View File

@ -16,30 +16,66 @@
extern void hal_init(void);
#define LED1 PIN('E', 15)
#define LED2 PIN('E', 4)
#define LED3 PIN('E', 0)
#define UART_DEBUG R_SCI0
#define UART_TX PIN('G', 13)
#define UART_RX PIN('G', 12)
#define BIT(x) (1UL << (x))
#define CLRSET(reg, clear, set) ((reg) = ((reg) & ~(clear)) | (set))
#define PIN(port, num) ((((port) - 'A') << 8) | (num))
#define SETBITS(reg, clear, set) ((reg) = ((reg) & ~(clear)) | (set))
#define PIN(port, num) ((((port) - '0') << 8) | (num))
#define PIN_NUM(pin) (pin & 255)
#define PIN_PORT(pin) (pin >> 8)
// No-op HAL API implementation for a device with GPIO and UART
#define LED1 PIN('4', 15) // blue
#define LED2 PIN('4', 4) // green
#define LED3 PIN('4', 0) // red
#define LED LED2 // Use green LED for blinking
#define rng_read() 0
#define rng_init()
#define uart_read_ready(uart) 0
#define GENERATE_MAC_ADDRESS() \
{ 2, 3, 4, 5, 6, 7 }
// Clock
#define SYS_FREQUENCY 2000000
// Board UM: P212 P213 24MHz xtal
// - (8.1 Table 8.1) (50.3.1 Table 50.14): PLL 120 ~ 200 MHz; input 8 ~ 24 MHz.
// System clock (ICLK) <= 200 MHz; PCLKA <= 100 MHz; PCLKB <= 50 MHz
// - (8.2.2): ICLK >= other clocks of interest for us
// - (8.7.3): FlashIF Clock (FCLK) (8.1 Table 8.1): 4 MHz to 50 MHz(P/E), up to
// 50 MHz (R)
// - (8.2.9): MOSCCR Set the PRCR.PRC0 bit to 1 (write enabled) before rewriting
// this register.
// - (10.2.9) OPCCR : Operating Power Control Register, defaults to high-speed
// mode
// - (45.2.4) 1 wait state for SRAM (default);
#define SYS_FREQUENCY 200000000
#define PLL_MUL 49 // x25 ((n+1)/2)
#define PLL_DIV 2 // /3 (n+1)
#define ICLK_DIV 0 // /1 -> 200MHz (/(2^n))
#define FCLK_DIV 2 // /4 -> 50MHz
#define PCLKAD_DIV 1 // /2 -> 100MHz
#define PCLKBC_DIV 2 // /4 -> 50MHz
#define FLASH_MHZ (SYS_FREQUENCY / ((1 << FCLK_DIV) * 1000000))
static inline void clock_init(void) {
SCB->CPACR = (uint32_t) 15U << 20;
R_SYSTEM->PRCR = 0xA501; // enable writing to osc control regs (12.2.1)
R_SYSTEM->MOSCWTCR = 9; // (8.2.5, 8.2.4)
R_SYSTEM->MOSCCR = 0; // enable main oscillator, default cfg (20~24MHz)
while (R_SYSTEM->OSCSF_b.MOSCSF == 0) (void) 0; // wait until it stabilizes
R_SYSTEM->PLLCCR =
(PLL_MUL << R_SYSTEM_PLLCCR_PLLMUL_Pos) |
(PLL_DIV
<< R_SYSTEM_PLLCCR_PLIDIV_Pos); // config PLL for MOSC /3 x 25 (8.2.4)
R_SYSTEM->PLLCR = 0; // enable PLL
R_SYSTEM->SCKDIVCR =
(FCLK_DIV << R_SYSTEM_SCKDIVCR_FCK_Pos) |
(ICLK_DIV << R_SYSTEM_SCKDIVCR_ICK_Pos) |
(PCLKAD_DIV << R_SYSTEM_SCKDIVCR_BCK_Pos) |
(PCLKAD_DIV << R_SYSTEM_SCKDIVCR_PCKA_Pos) |
(PCLKBC_DIV << R_SYSTEM_SCKDIVCR_PCKB_Pos) |
(PCLKBC_DIV << R_SYSTEM_SCKDIVCR_PCKC_Pos) |
(PCLKAD_DIV << R_SYSTEM_SCKDIVCR_PCKD_Pos); // set dividers
R_FCACHE->FLWT = 3; // flash: 3 wait states (47.4.3)
R_FLAD->FCKMHZ = FLASH_MHZ; // flash: read speed optimization (47.4.28)
R_FACI_HP->FPCKAR = (0x1E << 8) + FLASH_MHZ; // flash: write (47.4.26)
while (R_SYSTEM->OSCSF_b.PLLSF == 0) (void) 0; // PLL stabilization
while (R_FLAD->FCKMHZ != FLASH_MHZ) (void) 0; // flash module magic
R_SYSTEM->SCKSCR = 5; // select PLL (8.2.3)
}
// GPIO
@ -48,12 +84,22 @@ enum { GPIO_MODE_INPUT, GPIO_MODE_OUTPUT, GPIO_MODE_AF };
static inline void gpio_init(uint16_t pin, uint8_t mode, uint8_t af) {
R_PORT0_Type *gpio = GPIO(PIN_PORT(pin));
if (mode == GPIO_MODE_OUTPUT) {
gpio->PCNTR1 |= BIT(PIN_NUM(pin));
} else if (mode == GPIO_MODE_INPUT) {
gpio->PCNTR1 &= ~BIT(PIN_NUM(pin));
} else {
(void) af;
} else { // GPIO_MODE_AF
// TODO(): only non-analog supported (19.2.5)
R_PMISC->PWPR_b.B0WI = 0; // (19.5.1--> (RM says this is PFS...)
R_PMISC->PWPR_b.PFSWE = 1;
R_PFS->PORT[PIN_PORT(pin)].PIN[PIN_NUM(pin)].PmnPFS_b.PMR = 0;
R_PFS->PORT[PIN_PORT(pin)].PIN[PIN_NUM(pin)].PmnPFS_b.PSEL =
af & (R_PFS_PORT_PIN_PmnPFS_PSEL_Msk >> R_PFS_PORT_PIN_PmnPFS_PSEL_Pos);
R_PFS->PORT[PIN_PORT(pin)].PIN[PIN_NUM(pin)].PmnPFS_b.PMR = 1;
R_PFS->PORT[PIN_PORT(pin)].PIN[PIN_NUM(pin)].PmnPFS_b.DSCR = 2; // high
R_PMISC->PWPR_b.PFSWE = 0;
R_PMISC->PWPR_b.B0WI = 1; // <--19.5.1)
}
}
@ -79,49 +125,78 @@ static inline void gpio_toggle(uint16_t pin) {
gpio_write(pin, !gpio_read(pin));
}
#ifndef UART_DEBUG
#define UART_DEBUG R_SCI7
#endif
#define UART_TX PIN('6', 13) // SCI7
#define UART_RX PIN('6', 14)
// UART
// (datasheet, after BSP rev.eng.) SCI uses PCLKA in this chip
static inline void uart_init(R_SCI0_Type *uart, unsigned baud) {
gpio_output(UART_TX);
gpio_input(UART_RX);
if (uart != R_SCI7) return;
R_MSTP->MSTPCRB_b.MSTPB24 = 0; // enable SCI7
(void) R_MSTP->MSTPCRB; // (10.10.15)
uart->SCR = 0; // (29.3.7 -->) disable SCI
uart->FCR_b.FM = 0; // disable FIFO
uart->SIMR1 = 0; // disable I2C (default)
uart->SPMR = 0; // disable SPI (default)
uart->SCMR_b.SMIF = 0; // no smartcard (default)
uart->SMR = 0; // async 8N1, use PCLK
uart->SCMR_b.CHR1 = 1; // disable 9-bit mode (default)
uart->SEMR = 0; // 16x clocking (other SCIs need internal clock setting)
uart->SPTR = 3; // no inversions, high on idle
uart->BRR = (uint8_t) (SYS_FREQUENCY / (32 * (1 << PCLKAD_DIV) * baud));
gpio_init(UART_TX, GPIO_MODE_AF, 5); // (19.6)
gpio_init(UART_RX, GPIO_MODE_AF, 5);
(void) uart, (void) baud;
uart->SCR = R_SCI0_SCR_TE_Msk | R_SCI0_SCR_RE_Msk; // enable Tx, Rx
}
static inline void uart_write_byte(void *uart, uint8_t byte) {
uint16_t pin = UART_TX;
uint32_t baud = 9600;
uint32_t interval = SYS_FREQUENCY / baud; // Time to send 1 bit
uint8_t bits[] = {
0, // Start bit
(byte >> 0) & 1U,
(byte >> 1) & 1U,
(byte >> 2) & 1U,
(byte >> 3) & 1U,
(byte >> 4) & 1U,
(byte >> 5) & 1U,
(byte >> 6) & 1U,
(byte >> 7) & 1U,
1, // Stop bit
};
uint32_t t = SysTick->VAL; // Current timer value
for (uint8_t i = 0; i < 10; i++) {
gpio_write(pin, bits[i]);
if (t <= interval) {
while (SysTick->VAL <= t) (void) 0;
t = SysTick->LOAD + 1 - interval;
while (SysTick->VAL > t) (void) 0;
} else {
t -= interval;
while (SysTick->VAL > t) (void) 0;
}
}
(void) uart;
static inline void uart_write_byte(R_SCI0_Type *uart, uint8_t byte) {
uart->TDR = byte;
while (uart->SSR_b.TDRE == 0) (void) 0;
}
static inline void uart_write_buf(void *uart, char *buf, size_t len) {
static inline void uart_write_buf(R_SCI0_Type *uart, char *buf, size_t len) {
while (len-- > 0) uart_write_byte(uart, *(uint8_t *) buf++);
}
// Ethernet
static inline void ethernet_init(void) {
static inline int uart_read_ready(R_SCI0_Type *uart) {
return uart->SSR_b.RDRF; // If RDRF bit is set, data is ready
}
static inline uint8_t uart_read_byte(R_SCI0_Type *uart) {
return uart->RDR;
}
// Ethernet
// - (13.3.2 Table 13.4) Event table: Event 0x16F, ETHER_EINT0
// - (13.1) ICU (13.3.1) Interrupt Vector Table (13.5)
// - Choose a IELSRx, that is IRQn=x in NVIC, write event number
static inline void ethernet_init(void) {
R_MSTP->MSTPCRB_b.MSTPB15 = 0; // enable ETHERC0 and EDMAC0
(void) R_MSTP->MSTPCRB; // (10.10.15)
// (Board manual: 4.3.3 Table 1, 6.1 Table 20) (RM: 19.6)
R_SYSTEM->PRCR = 0xA502; // enable writing to next reg (12.2.1)
R_SYSTEM->VBTICTLR = 0; // enable MDC pins (19.5.5) (11.2.6)
gpio_init(PIN('4', 1), GPIO_MODE_AF, 0x17); // MDC
gpio_init(PIN('4', 2), GPIO_MODE_AF, 0x17); // MDIO
gpio_output(PIN('4', 3)); // PHY RST
gpio_write(PIN('0', 2), 1); // prevent NAND_TREE
gpio_write(PIN('4', 3), 0); // assert RST
gpio_init(PIN('4', 5), GPIO_MODE_AF, 0x17); // TX_EN
gpio_init(PIN('4', 6), GPIO_MODE_AF, 0x17); // TXD1
gpio_init(PIN('7', 0), GPIO_MODE_AF, 0x17); // TXD0
gpio_init(PIN('7', 1), GPIO_MODE_AF, 0x17); // REF50CK0
gpio_init(PIN('7', 2), GPIO_MODE_AF, 0x17); // RXD0
gpio_init(PIN('7', 3), GPIO_MODE_AF, 0x17); // RXD1
gpio_init(PIN('7', 4), GPIO_MODE_AF, 0x17); // RX_ER
gpio_init(PIN('7', 5), GPIO_MODE_AF, 0x17); // CRS_DV
R_PMISC->PFENET_b.PHYMODE0 = 0; // select RMII
for (volatile int i = 0; i < 0x2000; i++)
(void) 0; // keep PHY RST low for a while
gpio_write(PIN('4', 3), 1); // deassert RST
gpio_input(PIN('0', 2)); // PHY IRQ, not used
NVIC_EnableIRQ(0); // (13.5.1) no CMSIS support
R_ICU->IELSR[0] = 0x16f; // (13.2.15)(13.5.4.1)
}

View File

@ -8,15 +8,12 @@
#define BLINK_PERIOD_MS 1000 // LED blinking period in millis
static void timer_fn(void *arg) {
gpio_toggle(LED2); // Blink LED
#if 0
gpio_toggle(LED); // Blink LED
struct mg_tcpip_if *ifp = arg; // And show
const char *names[] = {"down", "up", "req", "ready"}; // network stats
MG_INFO(("Ethernet: %s, IP: %M, rx:%u, tx:%u, dr:%u, er:%u",
names[ifp->state], mg_print_ip4, &ifp->ip, ifp->nrecv, ifp->nsent,
ifp->ndrop, ifp->nerr));
#endif
(void) arg;
}
int main(void) {
@ -27,19 +24,19 @@ int main(void) {
mg_log_set(MG_LL_DEBUG); // Set log level
MG_INFO(("Starting, CPU freq %g MHz", (double) SystemCoreClock / 1000000));
mg_timer_add(&mgr, BLINK_PERIOD_MS, MG_TIMER_REPEAT, timer_fn, &mgr);
#if 0
// Initialise Mongoose network stack
struct mg_tcpip_driver_stm32h_data driver_data = {.mdc_cr = 4};
struct mg_tcpip_if mif = {.mac = GENERATE_MAC_ADDRESS(),
struct mg_tcpip_driver_ra_data driver_data = {
.clock = SystemCoreClock, .irqno = 0, .phy_addr = 1};
struct mg_tcpip_if mif = {// .mac = {...}, generate random mac address
// Uncomment below for static configuration:
// .ip = mg_htonl(MG_U32(192, 168, 0, 223)),
// .mask = mg_htonl(MG_U32(255, 255, 255, 0)),
// .gw = mg_htonl(MG_U32(192, 168, 0, 1)),
.driver = &mg_tcpip_driver_stm32h,
.driver = &mg_tcpip_driver_ra,
.driver_data = &driver_data};
mg_tcpip_init(&mgr, &mif);
mg_timer_add(&mgr, BLINK_PERIOD_MS, MG_TIMER_REPEAT, timer_fn, &mif);
MG_INFO(("MAC: %M. Waiting for IP...", mg_print_mac, mif.mac));
while (mif.state != MG_TCPIP_STATE_READY) {
@ -48,7 +45,6 @@ int main(void) {
MG_INFO(("Initialising application..."));
web_init(&mgr);
#endif
MG_INFO(("Starting event loop"));
for (;;) {

View File

@ -7,7 +7,7 @@
#define MG_ENABLE_TCPIP 1
#define MG_ENABLE_CUSTOM_MILLIS 1
#define MG_ENABLE_CUSTOM_RANDOM 1
#define MG_ENABLE_CUSTOM_RANDOM 0
#define MG_ENABLE_PACKED_FS 1
#define MG_ENABLE_DRIVER_STM32H 1
#define MG_ENABLE_DRIVER_RA 1
//#define MG_ENABLE_LINES 1

View File

@ -14403,14 +14403,14 @@ struct enet_desc {
};
// TODO(): handle these in a portable compiler-independent CMSIS-friendly way
#define MG_64BIT_ALIGNED __attribute__((aligned((64U))))
#define MG_64BYTE_ALIGNED __attribute__((aligned((64U))))
// Descriptors: in non-cached area (TODO(scaprile)), 64-bit aligned
// Buffers: 64-bit aligned
static volatile struct enet_desc s_rxdesc[ETH_DESC_CNT] MG_64BIT_ALIGNED;
static volatile struct enet_desc s_txdesc[ETH_DESC_CNT] MG_64BIT_ALIGNED;
static uint8_t s_rxbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BIT_ALIGNED;
static uint8_t s_txbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BIT_ALIGNED;
// Descriptors: in non-cached area (TODO(scaprile)), (37.5.1.22.2 37.5.1.23.2)
// Buffers: 64-byte aligned (37.3.14)
static volatile struct enet_desc s_rxdesc[ETH_DESC_CNT] MG_64BYTE_ALIGNED;
static volatile struct enet_desc s_txdesc[ETH_DESC_CNT] MG_64BYTE_ALIGNED;
static uint8_t s_rxbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BYTE_ALIGNED;
static uint8_t s_txbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BYTE_ALIGNED;
static struct mg_tcpip_if *s_ifp; // MIP interface
enum {
@ -14595,6 +14595,302 @@ struct mg_tcpip_driver mg_tcpip_driver_imxrt = {mg_tcpip_driver_imxrt_init,
#endif
#ifdef MG_ENABLE_LINES
#line 1 "src/drivers/ra.c"
#endif
#if MG_ENABLE_TCPIP && defined(MG_ENABLE_DRIVER_RA) && MG_ENABLE_DRIVER_RA
struct ra_etherc {
volatile uint32_t ECMR, RESERVED, RFLR, RESERVED1, ECSR, RESERVED2, ECSIPR,
RESERVED3, PIR, RESERVED4, PSR, RESERVED5[5], RDMLR, RESERVED6[3], IPGR,
APR, MPR, RESERVED7, RFCF, TPAUSER, TPAUSECR, BCFRR, RESERVED8[20], MAHR,
RESERVED9, MALR, RESERVED10, TROCR, CDCR, LCCR, CNDCR, RESERVED11, CEFCR,
FRECR, TSFRCR, TLFRCR, RFCR, MAFCR;
};
struct ra_edmac {
volatile uint32_t EDMR, RESERVED, EDTRR, RESERVED1, EDRRR, RESERVED2, TDLAR,
RESERVED3, RDLAR, RESERVED4, EESR, RESERVED5, EESIPR, RESERVED6, TRSCER,
RESERVED7, RMFCR, RESERVED8, TFTR, RESERVED9, FDR, RESERVED10, RMCR,
RESERVED11[2], TFUCR, RFOCR, IOSR, FCFTR, RESERVED12, RPADIR, TRIMD,
RESERVED13[18], RBWAR, RDFAR, RESERVED14, TBRAR, TDFAR;
};
#undef ETHERC
#define ETHERC ((struct ra_etherc *) (uintptr_t) 0x40114100U)
#undef EDMAC
#define EDMAC ((struct ra_edmac *) (uintptr_t) 0x40114000U)
#undef RASYSC
#define RASYSC ((uint32_t *) (uintptr_t) 0x4001E000U)
#undef ICU_IELSR
#define ICU_IELSR ((uint32_t *) (uintptr_t) 0x40006300U)
#define ETH_PKT_SIZE 1536 // Max frame size, multiple of 32
#define ETH_DESC_CNT 4 // Descriptors count
// TODO(): handle these in a portable compiler-independent CMSIS-friendly way
#define MG_16BYTE_ALIGNED __attribute__((aligned((16U))))
#define MG_32BYTE_ALIGNED __attribute__((aligned((32U))))
// Descriptors: 16-byte aligned
// Buffers: 32-byte aligned (27.3.1)
static volatile uint32_t s_rxdesc[ETH_DESC_CNT][4] MG_16BYTE_ALIGNED;
static volatile uint32_t s_txdesc[ETH_DESC_CNT][4] MG_16BYTE_ALIGNED;
static uint8_t s_rxbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_32BYTE_ALIGNED;
static uint8_t s_txbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_32BYTE_ALIGNED;
static struct mg_tcpip_if *s_ifp; // MIP interface
enum {
MG_PHYREG_BCR = 0,
MG_PHYREG_BSR = 1,
MG_PHYREG_ID1 = 2,
MG_PHYREG_ID2 = 3
};
// fastest is 3 cycles (SUB + BNE) on a 3-stage pipeline or equivalent
static inline void raspin(volatile uint32_t count) {
while (count--) (void) 0;
}
// count to get the 200ns SMC semi-cycle period (2.5MHz) calling raspin():
// SYS_FREQUENCY * 200ns / 3 = SYS_FREQUENCY / 15000000
static uint32_t s_smispin;
// Bit-banged SMI
static void smi_preamble(void) {
unsigned int i = 32;
uint32_t pir = MG_BIT(1) | MG_BIT(2); // write, mdio = 1, mdc = 0
ETHERC->PIR = pir;
while (i--) {
pir &= ~MG_BIT(0); // mdc = 0
ETHERC->PIR = pir;
raspin(s_smispin);
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
}
}
static void smi_wr(uint16_t header, uint16_t data) {
uint32_t word = (header << 16) | data;
smi_preamble();
unsigned int i = 32;
while (i--) {
uint32_t pir = MG_BIT(1) |
(word & 0x80000000 ? MG_BIT(2) : 0); // write, mdc = 0, data
ETHERC->PIR = pir;
raspin(s_smispin);
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
word <<= 1;
}
}
static uint16_t smi_rd(uint16_t header) {
smi_preamble();
unsigned int i = 16; // 2 LSb as turnaround
uint32_t pir;
while (i--) {
pir = (i > 1 ? MG_BIT(1) : 0) |
(header & 0x8000
? MG_BIT(2)
: 0); // mdc = 0, header, set read direction at turnaround
ETHERC->PIR = pir;
raspin(s_smispin);
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
header <<= 1;
}
i = 16;
uint16_t data = 0;
while (i--) {
data <<= 1;
pir = 0; // read, mdc = 0
ETHERC->PIR = pir;
raspin(s_smispin / 2); // 1/4 clock period, 300ns max access time
data |= ETHERC->PIR & MG_BIT(3) ? 1 : 0; // read mdio
raspin(s_smispin / 2); // 1/4 clock period
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
}
return data;
}
static uint16_t raeth_phy_read(uint8_t addr, uint8_t reg) {
return smi_rd((1 << 14) | (2 << 12) | (addr << 7) | (reg << 2) | (2 << 0));
}
static void raeth_phy_write(uint8_t addr, uint8_t reg, uint16_t val) {
smi_wr((1 << 14) | (1 << 12) | (addr << 7) | (reg << 2) | (2 << 0), val);
}
static uint32_t raeth_phy_id(uint8_t addr) {
uint16_t phy_id1 = raeth_phy_read(addr, MG_PHYREG_ID1);
uint16_t phy_id2 = raeth_phy_read(addr, MG_PHYREG_ID2);
return (uint32_t) phy_id1 << 16 | phy_id2;
}
// MDC clock is generated manually; as per 802.3, it must not exceed 2.5MHz
static bool mg_tcpip_driver_ra_init(struct mg_tcpip_if *ifp) {
struct mg_tcpip_driver_ra_data *d =
(struct mg_tcpip_driver_ra_data *) ifp->driver_data;
s_ifp = ifp;
// Init SMI clock timing. If user told us the clock value, use it.
// TODO(): Otherwise, guess
s_smispin = d->clock / 15000000;
// Init RX descriptors
for (int i = 0; i < ETH_DESC_CNT; i++) {
s_rxdesc[i][0] = MG_BIT(31); // RACT
s_rxdesc[i][1] = ETH_PKT_SIZE << 16; // RBL
s_rxdesc[i][2] = (uint32_t) s_rxbuf[i]; // Point to data buffer
}
s_rxdesc[ETH_DESC_CNT - 1][0] |= MG_BIT(30); // Wrap last descriptor
// Init TX descriptors
for (int i = 0; i < ETH_DESC_CNT; i++) {
// TACT = 0
s_txdesc[i][2] = (uint32_t) s_txbuf[i];
}
s_txdesc[ETH_DESC_CNT - 1][0] |= MG_BIT(30); // Wrap last descriptor
EDMAC->EDMR = MG_BIT(0); // Software reset, wait 64 PCLKA clocks (27.2.1)
uint32_t sckdivcr = RASYSC[8]; // get divisors from SCKDIVCR (8.2.2)
uint32_t ick = 1 << ((sckdivcr >> 24) & 7); // sys_clock div
uint32_t pcka = 1 << ((sckdivcr >> 12) & 7); // pclka div
raspin((64U * pcka) / (3U * ick));
EDMAC->EDMR = MG_BIT(6); // Initialize, little-endian (27.2.1)
MG_DEBUG(("PHY addr: %d, smispin: %d", d->phy_addr, s_smispin));
raeth_phy_write(d->phy_addr, MG_PHYREG_BCR, MG_BIT(15)); // Reset PHY
raeth_phy_write(d->phy_addr, MG_PHYREG_BCR,
MG_BIT(12)); // Set autonegotiation
// PHY: Enable ref clock (preserve defaults)
uint32_t id = raeth_phy_id(d->phy_addr);
MG_INFO(("PHY ID: %#04x %#04x", (uint16_t) (id >> 16), (uint16_t) id));
// 2000 a140 - TI DP83825I
// 0007 c0fx - LAN8720
// 0022 156x - KSZ8081RNB/KSZ8091RNB
if ((id & 0xffff0000) == 0x220000) { // KSZ8091RNB, like EK-RA6Mx boards
// 25 MHz xtal at XI/XO (default)
raeth_phy_write(d->phy_addr, 31, MG_BIT(15) | MG_BIT(8)); // PC2R
} else if ((id & 0xffff0000) == 0x20000000) { // DP83825I, like ???
// 50 MHz external at XI ???
raeth_phy_write(d->phy_addr, 23, 0x81); // 50MHz clock input
raeth_phy_write(d->phy_addr, 24, 0x280); // LED status, active high
} else { // Default to LAN8720
MG_INFO(("Defaulting to LAN8720 PHY...")); // TODO()
}
// Select RMII mode,
ETHERC->ECMR = MG_BIT(2) | MG_BIT(1); // 100M, Full-duplex, CRC
// ETHERC->ECMR |= MG_BIT(0); // Receive all
ETHERC->RFLR = 1518; // Set max rx length
EDMAC->RDLAR = (uint32_t) (uintptr_t) s_rxdesc;
EDMAC->TDLAR = (uint32_t) (uintptr_t) s_txdesc;
// MAC address filtering (bytes in reversed order)
ETHERC->MAHR = (uint32_t) (ifp->mac[0] << 24U) |
((uint32_t) ifp->mac[1] << 16U) |
((uint32_t) ifp->mac[2] << 8U) | ifp->mac[3];
ETHERC->MALR = ((uint32_t) ifp->mac[4] << 8U) | ifp->mac[5];
EDMAC->TFTR = 0; // Store and forward (27.2.10)
EDMAC->FDR = 0x070f; // (27.2.11)
EDMAC->RMCR = MG_BIT(0); // (27.2.12)
ETHERC->ECMR |= MG_BIT(6) | MG_BIT(5); // TE RE
EDMAC->EESIPR = MG_BIT(18); // Enable Rx IRQ
EDMAC->EDRRR = MG_BIT(0); // Receive Descriptors have changed
EDMAC->EDTRR = MG_BIT(0); // Transmit Descriptors have changed
return true;
}
// Transmit frame
static size_t mg_tcpip_driver_ra_tx(const void *buf, size_t len,
struct mg_tcpip_if *ifp) {
static int s_txno; // Current descriptor index
if (len > sizeof(s_txbuf[ETH_DESC_CNT])) {
ifp->nerr++;
MG_ERROR(("Frame too big, %ld", (long) len));
len = (size_t) -1; // fail
} else if ((s_txdesc[s_txno][0] & MG_BIT(31))) {
MG_ERROR(("No descriptors available"));
len = 0; // retry later
} else {
memcpy(s_txbuf[s_txno], buf, len); // Copy data
s_txdesc[s_txno][1] = len << 16; // Set data len
s_txdesc[s_txno][0] |= MG_BIT(31) | 3 << 28; // (27.3.1.1) mark valid
EDMAC->EDTRR = MG_BIT(0); // Transmit request
if (++s_txno >= ETH_DESC_CNT) s_txno = 0;
}
return len;
}
static bool mg_tcpip_driver_ra_up(struct mg_tcpip_if *ifp) {
struct mg_tcpip_driver_ra_data *d =
(struct mg_tcpip_driver_ra_data *) ifp->driver_data;
uint32_t bsr = raeth_phy_read(d->phy_addr, MG_PHYREG_BSR);
bool up = bsr & MG_BIT(2) ? 1 : 0;
if ((ifp->state == MG_TCPIP_STATE_DOWN) && up) { // link state just went up
// tmp = reg with flags set to the most likely situation: 100M full-duplex
// if(link is slow or half) set flags otherwise
// reg = tmp
uint32_t ecmr = ETHERC->ECMR | MG_BIT(2) | MG_BIT(1); // 100M Full-duplex
uint32_t phy_id = raeth_phy_id(d->phy_addr);
if ((phy_id & 0xffff0000) == 0x220000) { // KSZ8091RNB
uint16_t pc1r = raeth_phy_read(d->phy_addr, 30); // Read PC1R
if ((pc1r & 3) == 1) ecmr &= ~MG_BIT(2); // 10M
if ((pc1r & MG_BIT(2)) == 0) ecmr &= ~MG_BIT(1); // Half-duplex
} else if ((phy_id & 0xffff0000) == 0x20000000) { // DP83825I
uint16_t physts = raeth_phy_read(d->phy_addr, 16); // Read PHYSTS
if (physts & MG_BIT(1)) ecmr &= ~MG_BIT(2); // 10M
if ((physts & MG_BIT(2)) == 0) ecmr &= ~MG_BIT(1); // Half-duplex
} else { // Default to LAN8720
uint16_t scsr = raeth_phy_read(d->phy_addr, 31); // Read CSCR
if ((scsr & MG_BIT(3)) == 0) ecmr &= ~MG_BIT(2); // 10M
if ((scsr & MG_BIT(4)) == 0) ecmr &= ~MG_BIT(1); // Half-duplex
}
ETHERC->ECMR = ecmr; // IRQ handler does not fiddle with these registers
MG_DEBUG(("Link is %uM %s-duplex", ecmr & MG_BIT(2) ? 100 : 10,
ecmr & MG_BIT(1) ? "full" : "half"));
}
return up;
}
void EDMAC_IRQHandler(void);
static uint32_t s_rxno;
void EDMAC_IRQHandler(void) {
struct mg_tcpip_driver_ra_data *d =
(struct mg_tcpip_driver_ra_data *) s_ifp->driver_data;
EDMAC->EESR = MG_BIT(18); // Ack IRQ in EDMAC 1st
ICU_IELSR[d->irqno] &= ~MG_BIT(16); // Ack IRQ in ICU last
// Frame received, loop
for (uint32_t i = 0; i < 10; i++) { // read as they arrive but not forever
uint32_t r = s_rxdesc[s_rxno][0];
if (r & MG_BIT(31)) break; // exit when done
// skip partial/errored frames (27.3.1.2)
if ((r & (MG_BIT(29) | MG_BIT(28)) && !(r & MG_BIT(27)))) {
size_t len = s_rxdesc[s_rxno][1] & 0xffff;
mg_tcpip_qwrite(s_rxbuf[s_rxno], len, s_ifp); // CRC already stripped
}
s_rxdesc[s_rxno][0] |= MG_BIT(31);
if (++s_rxno >= ETH_DESC_CNT) s_rxno = 0;
}
EDMAC->EDRRR = MG_BIT(0); // Receive Descriptors have changed
// If b0 == 0, descriptors were exhausted and probably frames were dropped,
// (27.2.9 RMFCR counts them)
}
struct mg_tcpip_driver mg_tcpip_driver_ra = {mg_tcpip_driver_ra_init,
mg_tcpip_driver_ra_tx, NULL,
mg_tcpip_driver_ra_up};
#endif
#ifdef MG_ENABLE_LINES
#line 1 "src/drivers/same54.c"
#endif

View File

@ -2765,6 +2765,7 @@ extern struct mg_tcpip_driver mg_tcpip_driver_stm32h;
extern struct mg_tcpip_driver mg_tcpip_driver_imxrt;
extern struct mg_tcpip_driver mg_tcpip_driver_same54;
extern struct mg_tcpip_driver mg_tcpip_driver_cmsis;
extern struct mg_tcpip_driver mg_tcpip_driver_ra;
// Drivers that require SPI, can use this SPI abstraction
struct mg_tcpip_spi {
@ -2876,6 +2877,14 @@ struct mg_tcpip_driver_imxrt_data {
};
struct mg_tcpip_driver_ra_data {
// MDC clock "divider". MDC clock is software generated,
uint32_t clock; // core clock frequency in Hz
uint16_t irqno; // IRQn, R_ICU->IELSR[irqno]
uint8_t phy_addr; // PHY address
};
struct mg_tcpip_driver_same54_data {
int mdc_cr;
};

View File

@ -38,14 +38,14 @@ struct enet_desc {
};
// TODO(): handle these in a portable compiler-independent CMSIS-friendly way
#define MG_64BIT_ALIGNED __attribute__((aligned((64U))))
#define MG_64BYTE_ALIGNED __attribute__((aligned((64U))))
// Descriptors: in non-cached area (TODO(scaprile)), 64-bit aligned
// Buffers: 64-bit aligned
static volatile struct enet_desc s_rxdesc[ETH_DESC_CNT] MG_64BIT_ALIGNED;
static volatile struct enet_desc s_txdesc[ETH_DESC_CNT] MG_64BIT_ALIGNED;
static uint8_t s_rxbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BIT_ALIGNED;
static uint8_t s_txbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BIT_ALIGNED;
// Descriptors: in non-cached area (TODO(scaprile)), (37.5.1.22.2 37.5.1.23.2)
// Buffers: 64-byte aligned (37.3.14)
static volatile struct enet_desc s_rxdesc[ETH_DESC_CNT] MG_64BYTE_ALIGNED;
static volatile struct enet_desc s_txdesc[ETH_DESC_CNT] MG_64BYTE_ALIGNED;
static uint8_t s_rxbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BYTE_ALIGNED;
static uint8_t s_txbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_64BYTE_ALIGNED;
static struct mg_tcpip_if *s_ifp; // MIP interface
enum {

292
src/drivers/ra.c Normal file
View File

@ -0,0 +1,292 @@
#include "net_builtin.h"
#if MG_ENABLE_TCPIP && defined(MG_ENABLE_DRIVER_RA) && MG_ENABLE_DRIVER_RA
struct ra_etherc {
volatile uint32_t ECMR, RESERVED, RFLR, RESERVED1, ECSR, RESERVED2, ECSIPR,
RESERVED3, PIR, RESERVED4, PSR, RESERVED5[5], RDMLR, RESERVED6[3], IPGR,
APR, MPR, RESERVED7, RFCF, TPAUSER, TPAUSECR, BCFRR, RESERVED8[20], MAHR,
RESERVED9, MALR, RESERVED10, TROCR, CDCR, LCCR, CNDCR, RESERVED11, CEFCR,
FRECR, TSFRCR, TLFRCR, RFCR, MAFCR;
};
struct ra_edmac {
volatile uint32_t EDMR, RESERVED, EDTRR, RESERVED1, EDRRR, RESERVED2, TDLAR,
RESERVED3, RDLAR, RESERVED4, EESR, RESERVED5, EESIPR, RESERVED6, TRSCER,
RESERVED7, RMFCR, RESERVED8, TFTR, RESERVED9, FDR, RESERVED10, RMCR,
RESERVED11[2], TFUCR, RFOCR, IOSR, FCFTR, RESERVED12, RPADIR, TRIMD,
RESERVED13[18], RBWAR, RDFAR, RESERVED14, TBRAR, TDFAR;
};
#undef ETHERC
#define ETHERC ((struct ra_etherc *) (uintptr_t) 0x40114100U)
#undef EDMAC
#define EDMAC ((struct ra_edmac *) (uintptr_t) 0x40114000U)
#undef RASYSC
#define RASYSC ((uint32_t *) (uintptr_t) 0x4001E000U)
#undef ICU_IELSR
#define ICU_IELSR ((uint32_t *) (uintptr_t) 0x40006300U)
#define ETH_PKT_SIZE 1536 // Max frame size, multiple of 32
#define ETH_DESC_CNT 4 // Descriptors count
// TODO(): handle these in a portable compiler-independent CMSIS-friendly way
#define MG_16BYTE_ALIGNED __attribute__((aligned((16U))))
#define MG_32BYTE_ALIGNED __attribute__((aligned((32U))))
// Descriptors: 16-byte aligned
// Buffers: 32-byte aligned (27.3.1)
static volatile uint32_t s_rxdesc[ETH_DESC_CNT][4] MG_16BYTE_ALIGNED;
static volatile uint32_t s_txdesc[ETH_DESC_CNT][4] MG_16BYTE_ALIGNED;
static uint8_t s_rxbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_32BYTE_ALIGNED;
static uint8_t s_txbuf[ETH_DESC_CNT][ETH_PKT_SIZE] MG_32BYTE_ALIGNED;
static struct mg_tcpip_if *s_ifp; // MIP interface
enum {
MG_PHYREG_BCR = 0,
MG_PHYREG_BSR = 1,
MG_PHYREG_ID1 = 2,
MG_PHYREG_ID2 = 3
};
// fastest is 3 cycles (SUB + BNE) on a 3-stage pipeline or equivalent
static inline void raspin(volatile uint32_t count) {
while (count--) (void) 0;
}
// count to get the 200ns SMC semi-cycle period (2.5MHz) calling raspin():
// SYS_FREQUENCY * 200ns / 3 = SYS_FREQUENCY / 15000000
static uint32_t s_smispin;
// Bit-banged SMI
static void smi_preamble(void) {
unsigned int i = 32;
uint32_t pir = MG_BIT(1) | MG_BIT(2); // write, mdio = 1, mdc = 0
ETHERC->PIR = pir;
while (i--) {
pir &= ~MG_BIT(0); // mdc = 0
ETHERC->PIR = pir;
raspin(s_smispin);
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
}
}
static void smi_wr(uint16_t header, uint16_t data) {
uint32_t word = (header << 16) | data;
smi_preamble();
unsigned int i = 32;
while (i--) {
uint32_t pir = MG_BIT(1) |
(word & 0x80000000 ? MG_BIT(2) : 0); // write, mdc = 0, data
ETHERC->PIR = pir;
raspin(s_smispin);
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
word <<= 1;
}
}
static uint16_t smi_rd(uint16_t header) {
smi_preamble();
unsigned int i = 16; // 2 LSb as turnaround
uint32_t pir;
while (i--) {
pir = (i > 1 ? MG_BIT(1) : 0) |
(header & 0x8000
? MG_BIT(2)
: 0); // mdc = 0, header, set read direction at turnaround
ETHERC->PIR = pir;
raspin(s_smispin);
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
header <<= 1;
}
i = 16;
uint16_t data = 0;
while (i--) {
data <<= 1;
pir = 0; // read, mdc = 0
ETHERC->PIR = pir;
raspin(s_smispin / 2); // 1/4 clock period, 300ns max access time
data |= ETHERC->PIR & MG_BIT(3) ? 1 : 0; // read mdio
raspin(s_smispin / 2); // 1/4 clock period
pir |= MG_BIT(0); // mdc = 1
ETHERC->PIR = pir;
raspin(s_smispin);
}
return data;
}
static uint16_t raeth_phy_read(uint8_t addr, uint8_t reg) {
return smi_rd((1 << 14) | (2 << 12) | (addr << 7) | (reg << 2) | (2 << 0));
}
static void raeth_phy_write(uint8_t addr, uint8_t reg, uint16_t val) {
smi_wr((1 << 14) | (1 << 12) | (addr << 7) | (reg << 2) | (2 << 0), val);
}
static uint32_t raeth_phy_id(uint8_t addr) {
uint16_t phy_id1 = raeth_phy_read(addr, MG_PHYREG_ID1);
uint16_t phy_id2 = raeth_phy_read(addr, MG_PHYREG_ID2);
return (uint32_t) phy_id1 << 16 | phy_id2;
}
// MDC clock is generated manually; as per 802.3, it must not exceed 2.5MHz
static bool mg_tcpip_driver_ra_init(struct mg_tcpip_if *ifp) {
struct mg_tcpip_driver_ra_data *d =
(struct mg_tcpip_driver_ra_data *) ifp->driver_data;
s_ifp = ifp;
// Init SMI clock timing. If user told us the clock value, use it.
// TODO(): Otherwise, guess
s_smispin = d->clock / 15000000;
// Init RX descriptors
for (int i = 0; i < ETH_DESC_CNT; i++) {
s_rxdesc[i][0] = MG_BIT(31); // RACT
s_rxdesc[i][1] = ETH_PKT_SIZE << 16; // RBL
s_rxdesc[i][2] = (uint32_t) s_rxbuf[i]; // Point to data buffer
}
s_rxdesc[ETH_DESC_CNT - 1][0] |= MG_BIT(30); // Wrap last descriptor
// Init TX descriptors
for (int i = 0; i < ETH_DESC_CNT; i++) {
// TACT = 0
s_txdesc[i][2] = (uint32_t) s_txbuf[i];
}
s_txdesc[ETH_DESC_CNT - 1][0] |= MG_BIT(30); // Wrap last descriptor
EDMAC->EDMR = MG_BIT(0); // Software reset, wait 64 PCLKA clocks (27.2.1)
uint32_t sckdivcr = RASYSC[8]; // get divisors from SCKDIVCR (8.2.2)
uint32_t ick = 1 << ((sckdivcr >> 24) & 7); // sys_clock div
uint32_t pcka = 1 << ((sckdivcr >> 12) & 7); // pclka div
raspin((64U * pcka) / (3U * ick));
EDMAC->EDMR = MG_BIT(6); // Initialize, little-endian (27.2.1)
MG_DEBUG(("PHY addr: %d, smispin: %d", d->phy_addr, s_smispin));
raeth_phy_write(d->phy_addr, MG_PHYREG_BCR, MG_BIT(15)); // Reset PHY
raeth_phy_write(d->phy_addr, MG_PHYREG_BCR,
MG_BIT(12)); // Set autonegotiation
// PHY: Enable ref clock (preserve defaults)
uint32_t id = raeth_phy_id(d->phy_addr);
MG_INFO(("PHY ID: %#04x %#04x", (uint16_t) (id >> 16), (uint16_t) id));
// 2000 a140 - TI DP83825I
// 0007 c0fx - LAN8720
// 0022 156x - KSZ8081RNB/KSZ8091RNB
if ((id & 0xffff0000) == 0x220000) { // KSZ8091RNB, like EK-RA6Mx boards
// 25 MHz xtal at XI/XO (default)
raeth_phy_write(d->phy_addr, 31, MG_BIT(15) | MG_BIT(8)); // PC2R
} else if ((id & 0xffff0000) == 0x20000000) { // DP83825I, like ???
// 50 MHz external at XI ???
raeth_phy_write(d->phy_addr, 23, 0x81); // 50MHz clock input
raeth_phy_write(d->phy_addr, 24, 0x280); // LED status, active high
} else { // Default to LAN8720
MG_INFO(("Defaulting to LAN8720 PHY...")); // TODO()
}
// Select RMII mode,
ETHERC->ECMR = MG_BIT(2) | MG_BIT(1); // 100M, Full-duplex, CRC
// ETHERC->ECMR |= MG_BIT(0); // Receive all
ETHERC->RFLR = 1518; // Set max rx length
EDMAC->RDLAR = (uint32_t) (uintptr_t) s_rxdesc;
EDMAC->TDLAR = (uint32_t) (uintptr_t) s_txdesc;
// MAC address filtering (bytes in reversed order)
ETHERC->MAHR = (uint32_t) (ifp->mac[0] << 24U) |
((uint32_t) ifp->mac[1] << 16U) |
((uint32_t) ifp->mac[2] << 8U) | ifp->mac[3];
ETHERC->MALR = ((uint32_t) ifp->mac[4] << 8U) | ifp->mac[5];
EDMAC->TFTR = 0; // Store and forward (27.2.10)
EDMAC->FDR = 0x070f; // (27.2.11)
EDMAC->RMCR = MG_BIT(0); // (27.2.12)
ETHERC->ECMR |= MG_BIT(6) | MG_BIT(5); // TE RE
EDMAC->EESIPR = MG_BIT(18); // Enable Rx IRQ
EDMAC->EDRRR = MG_BIT(0); // Receive Descriptors have changed
EDMAC->EDTRR = MG_BIT(0); // Transmit Descriptors have changed
return true;
}
// Transmit frame
static size_t mg_tcpip_driver_ra_tx(const void *buf, size_t len,
struct mg_tcpip_if *ifp) {
static int s_txno; // Current descriptor index
if (len > sizeof(s_txbuf[ETH_DESC_CNT])) {
ifp->nerr++;
MG_ERROR(("Frame too big, %ld", (long) len));
len = (size_t) -1; // fail
} else if ((s_txdesc[s_txno][0] & MG_BIT(31))) {
MG_ERROR(("No descriptors available"));
len = 0; // retry later
} else {
memcpy(s_txbuf[s_txno], buf, len); // Copy data
s_txdesc[s_txno][1] = len << 16; // Set data len
s_txdesc[s_txno][0] |= MG_BIT(31) | 3 << 28; // (27.3.1.1) mark valid
EDMAC->EDTRR = MG_BIT(0); // Transmit request
if (++s_txno >= ETH_DESC_CNT) s_txno = 0;
}
return len;
}
static bool mg_tcpip_driver_ra_up(struct mg_tcpip_if *ifp) {
struct mg_tcpip_driver_ra_data *d =
(struct mg_tcpip_driver_ra_data *) ifp->driver_data;
uint32_t bsr = raeth_phy_read(d->phy_addr, MG_PHYREG_BSR);
bool up = bsr & MG_BIT(2) ? 1 : 0;
if ((ifp->state == MG_TCPIP_STATE_DOWN) && up) { // link state just went up
// tmp = reg with flags set to the most likely situation: 100M full-duplex
// if(link is slow or half) set flags otherwise
// reg = tmp
uint32_t ecmr = ETHERC->ECMR | MG_BIT(2) | MG_BIT(1); // 100M Full-duplex
uint32_t phy_id = raeth_phy_id(d->phy_addr);
if ((phy_id & 0xffff0000) == 0x220000) { // KSZ8091RNB
uint16_t pc1r = raeth_phy_read(d->phy_addr, 30); // Read PC1R
if ((pc1r & 3) == 1) ecmr &= ~MG_BIT(2); // 10M
if ((pc1r & MG_BIT(2)) == 0) ecmr &= ~MG_BIT(1); // Half-duplex
} else if ((phy_id & 0xffff0000) == 0x20000000) { // DP83825I
uint16_t physts = raeth_phy_read(d->phy_addr, 16); // Read PHYSTS
if (physts & MG_BIT(1)) ecmr &= ~MG_BIT(2); // 10M
if ((physts & MG_BIT(2)) == 0) ecmr &= ~MG_BIT(1); // Half-duplex
} else { // Default to LAN8720
uint16_t scsr = raeth_phy_read(d->phy_addr, 31); // Read CSCR
if ((scsr & MG_BIT(3)) == 0) ecmr &= ~MG_BIT(2); // 10M
if ((scsr & MG_BIT(4)) == 0) ecmr &= ~MG_BIT(1); // Half-duplex
}
ETHERC->ECMR = ecmr; // IRQ handler does not fiddle with these registers
MG_DEBUG(("Link is %uM %s-duplex", ecmr & MG_BIT(2) ? 100 : 10,
ecmr & MG_BIT(1) ? "full" : "half"));
}
return up;
}
void EDMAC_IRQHandler(void);
static uint32_t s_rxno;
void EDMAC_IRQHandler(void) {
struct mg_tcpip_driver_ra_data *d =
(struct mg_tcpip_driver_ra_data *) s_ifp->driver_data;
EDMAC->EESR = MG_BIT(18); // Ack IRQ in EDMAC 1st
ICU_IELSR[d->irqno] &= ~MG_BIT(16); // Ack IRQ in ICU last
// Frame received, loop
for (uint32_t i = 0; i < 10; i++) { // read as they arrive but not forever
uint32_t r = s_rxdesc[s_rxno][0];
if (r & MG_BIT(31)) break; // exit when done
// skip partial/errored frames (27.3.1.2)
if ((r & (MG_BIT(29) | MG_BIT(28)) && !(r & MG_BIT(27)))) {
size_t len = s_rxdesc[s_rxno][1] & 0xffff;
mg_tcpip_qwrite(s_rxbuf[s_rxno], len, s_ifp); // CRC already stripped
}
s_rxdesc[s_rxno][0] |= MG_BIT(31);
if (++s_rxno >= ETH_DESC_CNT) s_rxno = 0;
}
EDMAC->EDRRR = MG_BIT(0); // Receive Descriptors have changed
// If b0 == 0, descriptors were exhausted and probably frames were dropped,
// (27.2.9 RMFCR counts them)
}
struct mg_tcpip_driver mg_tcpip_driver_ra = {mg_tcpip_driver_ra_init,
mg_tcpip_driver_ra_tx, NULL,
mg_tcpip_driver_ra_up};
#endif

8
src/drivers/ra.h Normal file
View File

@ -0,0 +1,8 @@
#pragma once
struct mg_tcpip_driver_ra_data {
// MDC clock "divider". MDC clock is software generated,
uint32_t clock; // core clock frequency in Hz
uint16_t irqno; // IRQn, R_ICU->IELSR[irqno]
uint8_t phy_addr; // PHY address
};

View File

@ -59,6 +59,7 @@ extern struct mg_tcpip_driver mg_tcpip_driver_stm32h;
extern struct mg_tcpip_driver mg_tcpip_driver_imxrt;
extern struct mg_tcpip_driver mg_tcpip_driver_same54;
extern struct mg_tcpip_driver mg_tcpip_driver_cmsis;
extern struct mg_tcpip_driver mg_tcpip_driver_ra;
// Drivers that require SPI, can use this SPI abstraction
struct mg_tcpip_spi {