diff options
Diffstat (limited to 'firmware/x300/lib')
| -rw-r--r-- | firmware/x300/lib/CMakeLists.txt | 32 | ||||
| -rw-r--r-- | firmware/x300/lib/chinch.c | 310 | ||||
| -rw-r--r-- | firmware/x300/lib/ethernet.c | 639 | ||||
| -rw-r--r-- | firmware/x300/lib/link_state_route_proto.c | 441 | ||||
| -rw-r--r-- | firmware/x300/lib/mdelay.c | 36 | ||||
| -rw-r--r-- | firmware/x300/lib/print_addrs.c | 64 | ||||
| -rw-r--r-- | firmware/x300/lib/printf.c | 287 | ||||
| -rw-r--r-- | firmware/x300/lib/u3_net_stack.c | 589 | ||||
| -rw-r--r-- | firmware/x300/lib/udp_uart.c | 149 | ||||
| -rw-r--r-- | firmware/x300/lib/wb_i2c.c | 137 | ||||
| -rw-r--r-- | firmware/x300/lib/wb_pkt_iface64.c | 87 | ||||
| -rw-r--r-- | firmware/x300/lib/wb_uart.c | 34 |
12 files changed, 2805 insertions, 0 deletions
diff --git a/firmware/x300/lib/CMakeLists.txt b/firmware/x300/lib/CMakeLists.txt new file mode 100644 index 000000000..ed5a0a7c0 --- /dev/null +++ b/firmware/x300/lib/CMakeLists.txt @@ -0,0 +1,32 @@ +# +# Copyright 2010-2013 Ettus Research LLC +# +# This program is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# 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. +# +# You should have received a copy of the GNU General Public License +# along with this program. If not, see <http://www.gnu.org/licenses/>. +# + +######################################################################## + +add_library(usrpfw STATIC + udp_uart.c + wb_uart.c + wb_i2c.c + printf.c + wb_pkt_iface64.c + u3_net_stack.c + ethernet.c + mdelay.c + chinch.c + print_addrs.c + link_state_route_proto.c +) diff --git a/firmware/x300/lib/chinch.c b/firmware/x300/lib/chinch.c new file mode 100644 index 000000000..054845754 --- /dev/null +++ b/firmware/x300/lib/chinch.c @@ -0,0 +1,310 @@ +// +// Copyright 2013 Ettus Research LLC +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// 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. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see <http://www.gnu.org/licenses/>. +// + +#include "chinch.h" + +#define PCIE_MSG_REG_BASE 0xFB00 +#define PCIE_MSG_DATA_REG 0 //Write: Data register for outbound requests and responses + //Read: Latched data for inbound requests +#define PCIE_MSG_CTRL_REG 1 //Write: Control register for outbound requests and responses (Initiates xfer) + //Read: Latched control word for inbound requests +#define PCIE_MSG_RESP_REG 2 //Read: Latched response data for outbound requests +#define PCIE_MSG_STATUS_REG 3 //Read: Status register for inbound and outbound transactions + +// Transaction Word Format +// +// -Control- --Data-- +// 63 32 31 0 +// | | | | +// FXXA AAAA DDDD DDDD +// +// where: +// D = Data/Payload +// A = Address +// X = Reserved +// F = Flags: {Read_Response, Write_Request, Read_Request, Half_Word_Cycle} + +#define PCIE_CTRL_REG_READ_RESP (1<<31) +#define PCIE_CTRL_REG_WRITE (1<<30) +#define PCIE_CTRL_REG_READ (1<<29) +#define PCIE_CTRL_REG_HALF_WORD (1<<28) +#define PCIE_CTRL_REG_ADDR_MASK 0x000FFFFF + +#define PCIE_STATUS_REG_READ_PENDING (1<<0) +#define PCIE_STATUS_REG_REQ_PENDING (1<<1) +#define PCIE_STATUS_REG_RESP_PENDING (1<<2) +#define PCIE_STATUS_REG_BUSY (1<<4) + +#define CHINCH_FPGA_CONFIG_REG 0x58 +#define CHINCH_FLASH_WINDOW_REG0 0xC0 +#define CHINCH_FLASH_WINDOW_REG1 0xE0 +#define CHINCH_FLASH_2AAA_REG 0x400 +#define CHINCH_FLASH_5555_REG 0x408 +#define CHINCH_FLASH_WINDOW_BASE 0x60000 +#define CHINCH_FLASH_WINDOW_SIZE 0x20000 +#define CHINCH_FLASH_WINDOW_CONF 0x91 + +//----------------------------------------------------- +// Peek-Poke interface +//----------------------------------------------------- + +bool chinch_poke( + const uint32_t addr, + const uint32_t data, + bool half_word, + uint32_t timeout +) +{ + //Build transaction control word + uint32_t ctrl_word = 0, i; + ctrl_word |= (addr & PCIE_CTRL_REG_ADDR_MASK); + if (half_word) ctrl_word |= PCIE_CTRL_REG_HALF_WORD; + ctrl_word |= PCIE_CTRL_REG_WRITE; + + //Wait for space in the transaction queue or timeout + i = 0; + while ((wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_STATUS_REG)) & PCIE_STATUS_REG_BUSY) != 0) { + if (++i > timeout) return false; + } + + //Flush transaction control and data registers + wb_poke32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_DATA_REG), data); + wb_poke32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_CTRL_REG), ctrl_word); + + return true; +} + +bool chinch_peek( + const uint32_t addr, + uint32_t* data, + bool half_word, + uint32_t timeout +) +{ + //Build transaction control word + uint32_t ctrl_word = 0, i; + ctrl_word |= (addr & PCIE_CTRL_REG_ADDR_MASK); + if (half_word) ctrl_word |= PCIE_CTRL_REG_HALF_WORD; + ctrl_word |= PCIE_CTRL_REG_READ; + + //Wait for space in the transaction queue or timeout + i = 0; + while ((wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_STATUS_REG)) & PCIE_STATUS_REG_BUSY) != 0) { + if (++i > timeout) return false; + } + + //Flush transaction control register + if (data) *data = 0; + wb_poke32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_CTRL_REG), ctrl_word); + + //Wait for read completion or timeout + i = 0; + while ((wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_STATUS_REG)) & PCIE_STATUS_REG_READ_PENDING) != 0) { + if (++i > timeout) return false; + } + //Read transaction data register + if (data) *data = wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_RESP_REG)); + return true; +} + +//----------------------------------------------------- +// Flash access +//----------------------------------------------------- + +uint32_t g_cached_win_reg0; +uint32_t g_cached_win_reg1; + +bool chinch_flash_init() +{ + chinch_peek32(CHINCH_FLASH_WINDOW_REG0, &g_cached_win_reg0); + chinch_peek32(CHINCH_FLASH_WINDOW_REG1, &g_cached_win_reg1); + + bool status = true, passed = true; + STATUS_MERGE(chinch_poke32(CHINCH_FLASH_WINDOW_REG0, CHINCH_FLASH_WINDOW_BASE | CHINCH_FLASH_WINDOW_CONF), status); + + //Run a loopback test to ensure that we will not corrupt the flash. + STATUS_MERGE(chinch_poke32(0x200, 0xDEADBEEF), status); + STATUS_MERGE(chinch_poke16(0x204, 0x5678), status); + uint32_t reg_val; + STATUS_MERGE(chinch_peek16(0x0, ®_val), status); + STATUS_MERGE(chinch_poke16(0x206, reg_val), status); + STATUS_MERGE(chinch_peek32(0x200, ®_val), status); + passed &= (reg_val == 0xDEADBEEF); + STATUS_MERGE(chinch_peek32(0x204, ®_val), status); + passed &= (reg_val == 0x7AD05678); + + return status && passed; +} + +void chinch_flash_cleanup() +{ + chinch_poke32(CHINCH_FLASH_WINDOW_REG0, g_cached_win_reg0); + chinch_poke32(CHINCH_FLASH_WINDOW_REG1, g_cached_win_reg1); +} + +bool chinch_flash_select_sector(uint32_t sector) +{ + return chinch_poke32(CHINCH_FLASH_WINDOW_REG1, sector * CHINCH_FLASH_WINDOW_SIZE); +} + +bool chinch_flash_erase_sector() +{ + bool status = true; + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_5555_REG, 0x00AA), status); //Unlock #1 + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_2AAA_REG, 0x0055), status); //Unlock #2 + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_5555_REG, 0x0080), status); //Setup + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_5555_REG, 0x00AA), status); //Unlock #1 + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_2AAA_REG, 0x0055), status); //Unlock #2 + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_WINDOW_BASE, 0x0030), status); //Erase + + if (status) { + uint32_t read_data; + while (true) { + status = chinch_peek16(CHINCH_FLASH_WINDOW_BASE, &read_data); //Wait for sector to erase + if (((read_data & 0xFFFF) == 0xFFFF) || !status) break; + } + } + return status; +} + +bool chinch_flash_read_buf(uint32_t offset, uint16_t* buf, uint32_t size) +{ + bool status = true; + uint32_t base_addr = CHINCH_FLASH_WINDOW_BASE | (offset & 0x3FFFF); + for (uint32_t i = 0; (i < size) && status; i++) { + uint32_t word; + STATUS_CHAIN(chinch_peek16(base_addr + (i * 2), &word), status); + buf[i] = (uint16_t)word; + } + return status; +} + +bool chinch_flash_write_buf(uint32_t offset, uint16_t* buf, uint32_t size) +{ + if (size > CHINCH_FLASH_MAX_BUF_WRITES || buf == 0) return false; + bool status = true; + + //Setup buffered write + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_5555_REG, 0x00AA), status); //Unlock #1 + STATUS_MERGE(chinch_poke16(CHINCH_FLASH_2AAA_REG, 0x0055), status); //Unlock #2 + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_WINDOW_BASE, 0x0025), status); //Setup write + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_WINDOW_BASE, size - 1), status); //Num words + + //Write the data + uint32_t base_addr = CHINCH_FLASH_WINDOW_BASE | (offset & 0x3FFFF); + for (uint32_t i = 0; i < size; i++) { + STATUS_CHAIN(chinch_poke16(base_addr + (i * 2), buf[i]), status); + } + + //Commit write and poll until data is written + STATUS_CHAIN(chinch_poke16(CHINCH_FLASH_WINDOW_BASE, 0x0029), status); + if (status) { + uint16_t read_data; + while (true) { + STATUS_MERGE(chinch_flash_read(base_addr, &read_data), status); //Wait for write to finish + if ((read_data == buf[0]) || !status) break; + } + } + return status; +} + +//----------------------------------------------------- +// FPGA Configuration +//----------------------------------------------------- +void chinch_start_config() +{ + chinch_poke32(CHINCH_FPGA_CONFIG_REG, 0x1); +} + +config_status_t chinch_get_config_status() +{ + bool status = true; + uint32_t read_data; + STATUS_MERGE(chinch_peek32(CHINCH_FPGA_CONFIG_REG, &read_data), status); + return status ? (config_status_t)read_data : CHINCH_CONFIG_ERROR; +} + +//----------------------------------------------------- +// Read-back interface for the user initiated +// PCIe register transactions +//----------------------------------------------------- +pcie_register_xact_t g_pcie_reg_xact_info; +uint32_t g_pcie_res_timeout; + +bool _respond_to_pcie_xact_request(uint32_t response, uint32_t timeout) +{ + //Wait for space in the transaction queue or timeout + uint32_t i = 0; + while ((wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_STATUS_REG)) & PCIE_STATUS_REG_BUSY) != 0) { + if (++i > g_pcie_res_timeout) return false; + } + + //First write data and then the control register to ensure coherency + wb_poke32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_DATA_REG), response); + wb_poke32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_CTRL_REG), PCIE_CTRL_REG_READ_RESP); + + return true; +} + +bool check_pcie_user_regport(pcie_register_xact_t** xact_info_hdl) +{ + //Check for pending transaction requests + if ((wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_STATUS_REG)) & PCIE_STATUS_REG_REQ_PENDING) != 0) { + //Attach responder to transaction info + g_pcie_reg_xact_info.respond = _respond_to_pcie_xact_request; + + //First read data and then the control register to ensure coherency + g_pcie_reg_xact_info.data = wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_DATA_REG)); + uint32_t xact_control = wb_peek32(SR_ADDR(PCIE_MSG_REG_BASE, PCIE_MSG_CTRL_REG)); + + g_pcie_reg_xact_info.addr = xact_control & PCIE_CTRL_REG_ADDR_MASK; + g_pcie_reg_xact_info.size = + (xact_control & PCIE_CTRL_REG_HALF_WORD) == 0 ? PCIE_XACT_32_BIT : PCIE_XACT_16_BIT; + if ((xact_control & PCIE_CTRL_REG_READ) != 0) + g_pcie_reg_xact_info.type = PCIE_XACT_READ; + else if ((xact_control & PCIE_CTRL_REG_WRITE) != 0) + g_pcie_reg_xact_info.type = PCIE_XACT_WRITE; + else + g_pcie_reg_xact_info.type = PCIE_XACT_ERROR; + + *xact_info_hdl = &g_pcie_reg_xact_info; + return true; + } else { + *xact_info_hdl = 0; + return false; + } +} + +bool forward_pcie_user_xact_to_wb() +{ + pcie_register_xact_t* xact_info; + if (check_pcie_user_regport(&xact_info)) { + if (xact_info->size == PCIE_XACT_32_BIT) { + //Only respond to 32-bit transactions because that is all the LVFPGA interface can send + if (xact_info->type == PCIE_XACT_WRITE) { + wb_poke32(xact_info->addr, xact_info->data); + return true; + } else if (xact_info->type == PCIE_XACT_READ) { + return xact_info->respond(wb_peek32(xact_info->addr), CHINCH_DEFAULT_XACT_TIMEOUT); + } + } + } + return false; +} + + + diff --git a/firmware/x300/lib/ethernet.c b/firmware/x300/lib/ethernet.c new file mode 100644 index 000000000..806a3840d --- /dev/null +++ b/firmware/x300/lib/ethernet.c @@ -0,0 +1,639 @@ +/* + * Copyright 2007,2009 Free Software Foundation, Inc. + * Copyright 2009 Ettus Research LLC + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#ifdef HAVE_CONFIG_H +#include <config.h> +#endif +#include "../x300/x300_defs.h" +#include "ethernet.h" +#include "mdelay.h" +#include "printf.h" +#include "wb_i2c.h" +#include "wb_utils.h" +//#include "memory_map.h" +//#include "eth_phy.h" +//#include "pic.h" +//#include "hal_io.h" +//#include "nonstdio.h" +#include <stdint.h> +#include <stdbool.h> +#include "xge_phy.h" +#include "xge_mac.h" +#include <u3_net_stack.h> + + + + +#define VERBOSE 0 + +#define NETHS 2 // # of ethernet interfaces + +static bool links_up[NETHS] = {}; + +//////////////////////////////////////////////////////////////////////// +// +// 10 Gig Ethernet MAC. +// +typedef struct { + volatile uint32_t config; // WO + volatile uint32_t int_pending; // Clear-on-read + volatile uint32_t int_status; // RO + volatile uint32_t int_mask; // RW + volatile uint32_t mdio_data; + volatile uint32_t mdio_addr; + volatile uint32_t mdio_op; + volatile uint32_t mdio_control; + volatile uint32_t gpio; +} xge_regs_t; + +#define xge_regs ((xge_regs_t *) base) + +#define SFPP_STATUS_MODABS_CHG (1 << 5) // Has MODABS changed since last read? +#define SFPP_STATUS_TXFAULT_CHG (1 << 4) // Has TXFAULT changed since last read? +#define SFPP_STATUS_RXLOS_CHG (1 << 3) // Has RXLOS changed since last read? +#define SFPP_STATUS_MODABS (1 << 2) // MODABS state +#define SFPP_STATUS_TXFAULT (1 << 1) // TXFAULT state +#define SFPP_STATUS_RXLOS (1 << 0) // RXLOS state + + +int +ethernet_ninterfaces(void) +{ + return NETHS; +} + +//////////////////////////////////////////////////////////////////////// +// +// Clause 45 MDIO used for 10Gig Ethernet has two bus transactions to complete a transfer. +// An initial transaction sets up the address, and a subsequent one transfers the read or write data. +// +static uint32_t +xge_read_mdio(const uint32_t base, const uint32_t address, const uint32_t device, const uint32_t port) +{ + // Set register address each iteration + xge_regs->mdio_addr = address; + // Its a clause 45 device. We want to ADDRESS + xge_regs->mdio_op = XGE_MDIO_CLAUSE(CLAUSE45) | XGE_MDIO_OP(MDIO_ADDRESS) | XGE_MDIO_ADDR(port) | XGE_MDIO_MMD(device); + // Start MDIO bus transaction + xge_regs->mdio_control = 1; + // Wait until bus transaction complete + while (xge_regs->mdio_control == 1); + // Its a clause 45 device. We want to READ + xge_regs->mdio_op = XGE_MDIO_CLAUSE(CLAUSE45) | XGE_MDIO_OP(MDIO_READ) | XGE_MDIO_ADDR(port) | XGE_MDIO_MMD(device); + // Start MDIO bus transaction + xge_regs->mdio_control = 1; + // Wait until bus transaction complete + while (xge_regs->mdio_control == 1); + // Read MDIO data + return(xge_regs->mdio_data); +} + +static void +xge_write_mdio(const uint32_t base, const uint32_t address, const uint32_t device, const uint32_t port, const uint32_t data) +{ + // Set register address each iteration + xge_regs->mdio_addr = address; + // Its a clause 45 device. We want to ADDRESS + xge_regs->mdio_op = XGE_MDIO_CLAUSE(CLAUSE45) | XGE_MDIO_OP(MDIO_ADDRESS) | XGE_MDIO_ADDR(port) | XGE_MDIO_MMD(device); + // Start MDIO bus transaction + xge_regs->mdio_control = 1; + // Wait until bus transaction complete + while (xge_regs->mdio_control == 1); + // Write new value to mdio_write_data reg. + xge_regs->mdio_data = data; + // Its a clause 45 device. We want to WRITE + xge_regs->mdio_op = XGE_MDIO_CLAUSE(CLAUSE45) | XGE_MDIO_OP(MDIO_WRITE) | XGE_MDIO_ADDR(port) | XGE_MDIO_MMD(device); + // Start MDIO bus transaction + xge_regs->mdio_control = 1; + // Wait until bus transaction complete + while (xge_regs->mdio_control == 1); +} + +//////////////////////////////////////////////////////////////////////// +// +// Clause 22 MDIO used for 1Gig Ethernet has one bus transaction to complete a transfer. +// +static uint32_t +ge_read_mdio(const uint32_t base, const uint32_t address, const uint32_t port) +{ + // Its a clause 22 device. We want to READ + xge_regs->mdio_op = XGE_MDIO_CLAUSE(CLAUSE22) | XGE_MDIO_OP(MDIO_C22_READ) | XGE_MDIO_ADDR(port) | address; + // Start MDIO bus transaction + xge_regs->mdio_control = 1; + // Wait until bus transaction complete + while (xge_regs->mdio_control == 1); + // Read MDIO data + return(xge_regs->mdio_data); +} + +static void +ge_write_mdio(const uint32_t base, const uint32_t address, const uint32_t port, const uint32_t data) +{ + // Write new value to mdio_write_data reg. + xge_regs->mdio_data = data; + // Its a clause 22 device. We want to WRITE + xge_regs->mdio_op = XGE_MDIO_CLAUSE(CLAUSE22) | XGE_MDIO_OP(MDIO_C22_WRITE) | XGE_MDIO_ADDR(port) | address; + // Start MDIO bus transaction + xge_regs->mdio_control = 1; + // Wait until bus transaction complete + while (xge_regs->mdio_control == 1); +} + +//////////////////////////////////////////////////////////////////////// +// +// Read and write MDIO independent of type +// + +static uint32_t read_mdio(const uint8_t eth, const uint32_t address, const uint32_t device, const uint32_t port) +{ + const uint32_t rb_addr = (eth==0) ? RB_ETH_TYPE0 : RB_ETH_TYPE1; + const uint32_t base = (eth==0) ? XGE0_BASE : XGE1_BASE; + if (wb_peek32(SR_ADDR(RB0_BASE, rb_addr)) != 0) + { + return xge_read_mdio(base, address, device, port); + } + else + { + return ge_read_mdio(base, address, port); + } +} + +static void write_mdio(const uint8_t eth, const uint32_t address, const uint32_t device, const uint32_t port, const uint32_t data) +{ + const uint32_t rb_addr = (eth==0) ? RB_ETH_TYPE0 : RB_ETH_TYPE1; + const uint32_t base = (eth==0) ? XGE0_BASE : XGE1_BASE; + if (wb_peek32(SR_ADDR(RB0_BASE, rb_addr)) != 0) + { + return xge_write_mdio(base, address, device, port, data); + } + else + { + return ge_write_mdio(base, address, port, data); + } +} + +//////////////////////////////////////////////////////////////////////// +// +// Read an 8-bit word from a device attached to the PHY's i2c bus. +// +static int +xge_i2c_rd(const uint32_t base, const uint8_t i2c_dev_addr, const uint8_t i2c_word_addr) +{ + uint8_t buf; + // IJB. CHECK HERE FOR MODET. Bail immediately if no module + + // SFF-8472 defines a hardcoded bus address of 0xA0, an 8bit internal address and a register map. + // Write the random access address to the SPF module + if (wb_i2c_write(base, i2c_dev_addr, &i2c_word_addr, 1) == false) + return(-1); + + // Now read back a byte of data + if (wb_i2c_read(base, i2c_dev_addr, &buf, 1) == false) + return(-1); + + return((int) buf); +} + +//////////////////////////////////////////////////////////////////////// +// +// Read identity of SFP+ module for XGE PHY +// +// (base is i2c controller) +static int +xge_read_sfpp_type(const uint32_t base, const uint32_t delay_ms) +{ + int x; + // Delay read of SFPP + if (delay_ms) + mdelay(delay_ms); + // Read ID code from SFP + x = xge_i2c_rd(base, MODULE_DEV_ADDR, 3); + // I2C Error? + if (x < 0) { + printf("DEBUG: I2C error in SFPP_TYPE.\n"); + return x; + } + // Decode module type. These registers and values are defined in SFF-8472 + if (x & 0x01) // Active 1X Infinband Copper + { + goto twinax; + } + if (x & 0x10) + { + printf("DEBUG: SFFP_TYPE_SR.\n"); + return SFFP_TYPE_SR; + } + if (x & 0x20) + { + printf("DEBUG: SFFP_TYPE_LR.\n"); + return SFFP_TYPE_LR; + } + if (x & 0x40) + { + printf("DEBUG: SFFP_TYPE_LRM.\n"); + return SFFP_TYPE_LRM; + } + // Search for legacy 1000-Base SFP types + x = xge_i2c_rd(base, MODULE_DEV_ADDR, 0x6); + if (x < 0) { + printf("DEBUG: I2C error in SFPP_TYPE.\n"); + return x; + } + if (x & 0x01) { + printf("DEBUG: SFFP_TYPE_1000BASE_SX.\n"); + return SFFP_TYPE_1000BASE_SX; + } + if (x & 0x02) { + printf("DEBUG: SFFP_TYPE_1000BASE_LX.\n"); + return SFFP_TYPE_1000BASE_LX; + } + if (x & 0x08) { + printf("DEBUG: SFFP_TYPE_1000BASE_T.\n"); + return SFFP_TYPE_1000BASE_T; + } + // Not one of the standard optical types..now try to deduce if it's twinax aka 10GSFP+CU + // which is not covered explicitly in SFF-8472 + x = xge_i2c_rd(base, MODULE_DEV_ADDR, 8); + if (x < 0) { + printf("DEBUG: I2C error in SFPP_TYPE.\n"); + return x; + } + if ((x & 4) == 0) // Passive SFP+ cable type + goto unknown; +// x = xge_i2c_rd(MODULE_DEV_ADDR, 6); +// printf("SFP+ reg6 read as %x\n",x); +// if (x < 0) +// return x; +// if (x != 0x04) // Returns 1000Base-CX as Compliance code +// goto unknown; + x = xge_i2c_rd(base, MODULE_DEV_ADDR, 0xA); + if (x < 0) { + printf("DEBUG: I2C error in SFPP_TYPE.\n"); + return x; + } + if (x & 0x80) { + twinax: + // Reports 1200 MBytes/sec fibre channel speed..close enough to 10G ethernet! + x = xge_i2c_rd(base, MODULE_DEV_ADDR, 0x12); + + if (x < 0) { + printf("DEBUG: I2C error in SFPP_TYPE.\n"); + return x; + } + printf("DEBUG: TwinAx.\n"); + // If cable length support is greater than 10M then pick correct type + return x > 10 ? SFFP_TYPE_TWINAX_LONG : SFFP_TYPE_TWINAX; + } +unknown: + printf("DEBUG: Unknown SFP+ type.\n"); + // Not a supported Module type + return SFFP_TYPE_UNKNOWN; +} + + +// Pull reset line low for 100ms then release and wait 100ms +static void +xge_hard_phy_reset(const uint32_t base) +{ + wb_poke32(base, 1); + mdelay(100); + wb_poke32(base, 0); + mdelay(100); + +} + +static void +xge_mac_init(const uint32_t base) +{ + printf("INFO: Begining XGE MAC init sequence.\n"); + xge_regs->config = XGE_TX_ENABLE; +} + +// base is pointer to XGE MAC on Wishbone. +static void +xge_phy_init(const uint8_t eth, const uint32_t mdio_port) +{ + int x; + // Read LASI Ctrl register to capture state. + //y = xge_read_mdio(0x9002,XGE_MDIO_DEVICE_PMA,XGE_MDIO_ADDR_PHY_A); + printf("INFO: Begining XGE PHY init sequence.\n"); + // Software reset + x = read_mdio(eth, 0x0, XGE_MDIO_DEVICE_PMA,mdio_port); + x = x | (1 << 15); + write_mdio(eth, 0x0,XGE_MDIO_DEVICE_PMA,mdio_port,x); + //FIXME uncomment lines below when 1gigE MDIO works + //while(x&(1<<15)) + // x = read_mdio(eth, 0x0,XGE_MDIO_DEVICE_PMA,mdio_port); +} + +void +xge_poll_sfpp_status(const uint32_t eth) +{ + uint32_t x; + // Has MODDET/MODAbS changed since we last looked? + x = wb_peek32(SR_ADDR(RB0_BASE, (eth==0) ? RB_SFPP_STATUS0 : RB_SFPP_STATUS1 )); + + if (x & SFPP_STATUS_RXLOS_CHG) + printf("DEBUG: eth%1d RXLOS changed state: %d\n", eth, x & SFPP_STATUS_RXLOS); + if (x & SFPP_STATUS_TXFAULT_CHG) + printf("DEBUG: eth%1d TXFAULT changed state: %d\n", eth,(x & SFPP_STATUS_TXFAULT) >> 1 ); + if (x & SFPP_STATUS_MODABS_CHG) + printf("DEBUG: eth%1d MODABS changed state: %d\n", eth, (x & SFPP_STATUS_MODABS) >> 2); + + if (x & (SFPP_STATUS_RXLOS_CHG|SFPP_STATUS_TXFAULT_CHG|SFPP_STATUS_MODABS_CHG)) + if (( x & (SFPP_STATUS_RXLOS|SFPP_STATUS_TXFAULT|SFPP_STATUS_MODABS)) == 0) { + xge_ethernet_init(eth); + dump_mdio_regs((eth==0) ? XGE0_BASE : XGE1_BASE,MDIO_PORT); + mdelay(100); + dump_mdio_regs((eth==0) ? XGE0_BASE : XGE1_BASE,MDIO_PORT); + mdelay(100); + dump_mdio_regs((eth==0) ? XGE0_BASE : XGE1_BASE,MDIO_PORT); + } + + if (x & SFPP_STATUS_MODABS_CHG) { + // MODDET has changed state since last checked + if (x & SFPP_STATUS_MODABS) { + // MODDET is high, module currently removed. + printf("INFO: An SFP+ module has been removed from eth port %d.\n", eth); + } else { + // MODDET is low, module currently inserted. + // Return status. + printf("INFO: A new SFP+ module has been inserted into eth port %d.\n", eth); + xge_read_sfpp_type((eth==0) ? I2C0_BASE : I2C2_BASE,1); + } + } + + //update the link up status + const bool old_link_up = links_up[eth]; + links_up[eth] = ((read_mdio(eth, XGE_MDIO_STATUS1,XGE_MDIO_DEVICE_PMA,MDIO_PORT)) & (1 << 2)) != 0; + //The link became up, send a GARP so everyone knows our mac/ip association + if (!old_link_up && links_up[eth]) u3_net_stack_send_arp_request(eth, u3_net_stack_get_ip_addr(eth)); +} + + +void +xge_ethernet_init(const uint32_t eth) +{ + xge_mac_init((eth==0) ? XGE0_BASE : XGE1_BASE); + //xge_hard_phy_reset(); + xge_phy_init(eth ,MDIO_PORT); + uint32_t x = wb_peek32(SR_ADDR(RB0_BASE, (eth==0) ? RB_SFPP_STATUS0 : RB_SFPP_STATUS1 )); + printf(" eth%1d SFP initial state: RXLOS: %d TXFAULT: %d MODABS: %d\n", + eth, + x & SFPP_STATUS_RXLOS, + (x & SFPP_STATUS_TXFAULT) >> 1, + (x & SFPP_STATUS_MODABS) >> 2); +} + +// +// Debug code to verbosely read XGE MDIO registers below here. +// + + +void decode_reg(uint32_t address, uint32_t device, uint32_t data) +{ + int x; + printf("Device: "); + printf("%x",device); + printf(" "); + switch(address) { + case XGE_MDIO_CONTROL1: + printf("CONTROL1: "); + printf("%x",data); printf(" "); + for (x=15; x >= 0 ; x--) + if ((data & (1 << x)) != 0) + // Bits set. + switch(x) { + case 15: printf("Reset,"); break; + case 14: printf("Loopback,"); break; + case 11: printf("Low Power Mode,"); break; + case 5:case 4:case 3:case 2: printf("RESERVED speed value,"); break; + case 0: printf("PMA loopback,"); break; + } //else + // Bits clear. + //switch (x) { + //case 13: case 6: printf(" None 10Gb/s speed set!"); break; + //} + printf(" \n"); + break; + case XGE_MDIO_STATUS1: + printf("STATUS1: "); + printf("%x",data); printf(" "); + for (x=15; x >= 0 ; x--) + if ((data & (1 << x)) != 0) + // Bits set. + switch(x) { + case 7: printf("Fault Detected,"); break; + case 2: printf("Link is Up,"); break; + case 1: printf("Supports Low Power,"); break; + } else + // Bits Clear + switch(x) { + case 2: printf("Link is Down,"); break; + } + printf(" \n"); + break; + case XGE_MDIO_SPEED: + printf("SPEED ABILITY: "); + printf("%x",data); printf(" "); + for (x=15; x >= 0 ; x--) + if ((data & (1 << x)) != 0) + // Bits set. + switch(x) { + case 15:case 14:case 13:case 12:case 11:case 10:case 9: + case 8:case 7:case 6:case 5:case 4:case 3:case 2:case 1: printf("RESERVED bits set!,"); break; + case 0: printf("Capable of 10Gb/s,"); + } else + // Bits clear. + switch(x) { + case 0: printf("Incapable of 10Gb/s,"); break; + } + printf(" \n"); + break; + case XGE_MDIO_DEVICES1: + printf("DEVICES IN PACKAGE: "); + printf("%x",data); printf(" "); + for (x=15; x >= 0 ; x--) + if ((data & (1 << x)) != 0) + // Bits set. + switch(x) { + case 7: printf("Auto-Negotiation,"); break; + case 6: printf("TC,"); break; + case 5: printf("DTE XS,"); break; + case 4: printf("PHY XS,"); break; + case 3: printf("PCS,"); break; + case 2: printf("WIS,"); break; + case 1: printf("PMD/PMA,"); break; + case 0: printf("Clause 22 registers,"); break; + } + printf(" \n"); + break; + case XGE_MDIO_DEVICES2: + printf("DEVICES IN PACKAGE (cont): "); + printf("%x",data); printf(" "); + for (x=15; x >= 0 ; x--) + if ((data & (1 << x)) != 0) + // Bits set. + switch(x) { + case 15: printf("Vendor device 2,"); break; + case 14: printf("Vendor device 1,"); break; + case 13: printf("Clause 22 extension,"); break; + } + printf(" \n"); + break; + case XGE_MDIO_CONTROL2: + printf("CONTROL2: "); + printf("%x",data); printf(" "); + // PMA/PMD + if (device == XGE_MDIO_DEVICE_PMA) + switch((data & 0xf)) { + case 0xF: printf("10BASE-T,"); break; + case 0xE: printf("100BASE-TX,"); break; + case 0xD: printf("1000BASE-KX,"); break; + case 0xC: printf("1000BASE-T,"); break; + case 0xB: printf("10GBASE-KR,"); break; + case 0xA: printf("10GBASE-KX4,"); break; + case 0x9: printf("10GBASE-T,"); break; + case 0x8: printf("10GBASE-LRM,"); break; + case 0x7: printf("10GBASE-SR,"); break; + case 0x6: printf("10GBASE-LR,"); break; + case 0x5: printf("10GBASE-ER,"); break; + case 0x4: printf("10GBASE-LX4,"); break; + // case 0x3: printf("10GBASE-SW,"); break; + // case 0x2: printf("10GBASE-LW,"); break; + // case 0x1: printf("10GBASE-EW,"); break; + case 0x0: printf("10GBASE-CX4,"); break; + } else if (device == XGE_MDIO_DEVICE_PCS) + // PCS + switch((data & 0x3)) { + case 0x3: printf("10GBASE-T PCS,"); break; + case 0x2: printf("10GBASE-W PCS,"); break; + case 0x1: printf("10GBASE-X PCS,"); break; + case 0x0: printf("10GBASE-R PCS,"); break; + } + printf(" \n"); + break; + case XGE_MDIO_STATUS2: + printf("STATUS2: "); + printf("%x",data); printf(" "); + for (x=15; x >= 0 ; x--) + if ((data & (1 << x)) != 0) + // Bits set. + switch(x) { + case 15: if ((data & (1 << 14)) == 0) printf("Device responding,"); break; + case 13: if (device == XGE_MDIO_DEVICE_PMA) printf("Able detect a Tx fault,"); break; + case 12: if (device == XGE_MDIO_DEVICE_PMA) printf("Able detect an Rx fault,"); break; + case 11: printf("Fault on Tx path,"); break; + case 10: printf("Fault on Rx path,"); break; + case 9: if (device == XGE_MDIO_DEVICE_PMA) printf("Extended abilities in Reg1.11,"); break; + case 8: if (device == XGE_MDIO_DEVICE_PMA) printf("Able to disable TX,"); break; + case 7: if (device == XGE_MDIO_DEVICE_PMA) printf("10GBASE-SR,"); break; + case 6: if (device == XGE_MDIO_DEVICE_PMA) printf("10GBASE-LR,"); break; + case 5: if (device == XGE_MDIO_DEVICE_PMA) printf("10GBASE-ER,"); break; + case 4: if (device == XGE_MDIO_DEVICE_PMA) printf("10GBASE-LX4,"); break; + case 3: if (device == XGE_MDIO_DEVICE_PMA) printf("10GBASE-SW,"); break; + case 2: if (device == XGE_MDIO_DEVICE_PMA) printf("10GBASE-LW,"); break; + case 1: if (device == XGE_MDIO_DEVICE_PMA) printf("10GBASE-EW,"); break; + case 0: if (device == XGE_MDIO_DEVICE_PMA) printf("loopback,"); break; + } + printf(" \n"); + break; + case XGE_MDIO_LANESTATUS: + printf("LANE STATUS: "); + printf("%x",data); printf(" "); + for (x=15; x >= 0 ; x--) + if ((data & (1 << x)) != 0) + // Bits set. + switch(x) { + case 12: printf("Lanes aligned,"); break; + case 11: printf("Able to generate test patterns,"); break; + case 3: printf("Lane 3 synced,"); break; + case 2: printf("Lane 2 synced,"); break; + case 1: printf("Lane 1 synced,"); break; + case 0: printf("Lane 0 synced,"); break; + } else + // Bits clear + switch(x) { + case 3: printf("Lane 3 not synced,"); break; + case 2: printf("Lane 2 not synced,"); break; + case 1: printf("Lane 1 not synced,"); break; + case 0: printf("Lane 0 not synced,"); break; + } + printf(" \n"); + break; + case XILINX_CORE_VERSION: + printf("XILINX CORE VERSION: %x ",data); + printf("Version: %d.%d ",(data&0xf000)>>12,(data&0xf00)>>8); + printf("Patch: %d ",(data&0xE)>>1); + if (data&0x1) printf("Evaluation Version of core"); + printf("\n"); + break; + default: + printf("Register @ address: "); + printf("%x",address); + printf(" has value: "); + printf("%x\n",data); + break; + } +} + +void +dump_mdio_regs(const uint8_t eth, uint32_t mdio_port) +{ + volatile unsigned int x; + int y; + unsigned int regs_a[9] = {0,1,4,5,6,7,8,32,33}; + unsigned int regs_b[10] = {0,1,4,5,6,7,8,10,11,65535}; + + printf("\n"); + + for (y = 0; y < 10; y++) + { + // Read MDIO data + x = read_mdio(eth,regs_b[y],XGE_MDIO_DEVICE_PMA,mdio_port); + decode_reg(regs_b[y],XGE_MDIO_DEVICE_PMA,x); + } + + for (y = 0; y < 9; y++) + { + // Read MDIO data + x = read_mdio(eth,regs_a[y],XGE_MDIO_DEVICE_PCS,mdio_port); + decode_reg(regs_a[y],XGE_MDIO_DEVICE_PCS,x); + } + + printf("\n"); + + /* for (y = 0; y < 8; y++) */ + /* { */ + /* // Read MDIO data */ + /* x = xge_read_mdio(base,regs_a[y],XGE_MDIO_DEVICE_PHY_XS,mdio_port); */ + /* decode_reg(regs_a[y],XGE_MDIO_DEVICE_PHY_XS,x); */ + /* } */ + + /* for (y = 0; y < 8; y++) */ + /* { */ + /* // Read MDIO data */ + /* x = xge_read_mdio(base,regs_a[y],XGE_MDIO_DEVICE_DTE_XS,mdio_port); */ + /* decode_reg(regs_a[y],XGE_MDIO_DEVICE_DTE_XS,x); */ + /* } */ +} + +bool ethernet_get_link_up(const uint32_t eth) +{ + return links_up[eth]; +} diff --git a/firmware/x300/lib/link_state_route_proto.c b/firmware/x300/lib/link_state_route_proto.c new file mode 100644 index 000000000..30cfd73cb --- /dev/null +++ b/firmware/x300/lib/link_state_route_proto.c @@ -0,0 +1,441 @@ + +// Copyright 2013 Ettus Research LLC + +#include <link_state_route_proto.h> +#include <u3_net_stack.h> +#include <ethernet.h> +#include <string.h> +#include <printf.h> +#include <print_addrs.h> + +#define lengthof(a) (sizeof(a)/sizeof(*(a))) + +/*********************************************************************** + * global constants + **********************************************************************/ +#define LS_PROTO_VERSION 6 + +//shift the proto version into the ID so only matching fw responds +#define LS_ID_DISCOVER (0 | (8 << LS_PROTO_VERSION)) +#define LS_ID_INFORM (1 | (8 << LS_PROTO_VERSION)) + +#define LS_PAYLOAD_MTU 1024 +#define LS_NUM_NBOR_ENTRIES 16 +#define LS_NUM_NODE_ENTRIES 64 +#define LS_NUM_MAP_ENTRIES 128 + +#define NETHS 4 //max eths supported in this file + +/*********************************************************************** + * wire format for table communication + **********************************************************************/ +typedef struct +{ + uint32_t num_nbors; //number of entries in neighbors list + uint32_t num_ports; //first few neighbors are local ports + struct ip_addr node; + struct ip_addr nbors[]; +} ls_data_t; + +static inline size_t sizeof_ls_data(const ls_data_t *ls_data) +{ + return 0 + + sizeof(uint32_t)/*num neighbors*/ + + sizeof(uint32_t)/*num ports*/ + + sizeof(struct ip_addr)/*source node*/ + + sizeof(struct ip_addr)*ls_data->num_nbors; +} + +/*********************************************************************** + * sequence and tick counter monitor + **********************************************************************/ +static uint16_t ticker = 0; + +void link_state_route_proto_tick(void) +{ + ticker++; +} + +static inline bool is_tick_expired(const uint16_t tick) +{ + const uint16_t delta = ticker - tick; + return delta > 2; //have not talked in a while, you are deaf to me +} + +static uint16_t current_seq = 0; + +static inline bool is_seq_newer(const uint16_t seq, const uint16_t entry_seq) +{ + if (seq == entry_seq) return false; //not newer if equal + const uint16_t delta = seq - entry_seq; + return (delta & (1 << 15)) == 0; //newer when subtraction did not overflow +} + +/*********************************************************************** + * node entry api + **********************************************************************/ +typedef struct +{ + uint16_t seq; + uint16_t tick; + uint8_t ethno; + struct ip_addr ip_addr; +} ls_node_entry_t; + +static bool ls_node_entry_valid(const ls_node_entry_t *entry) +{ + return entry->ip_addr.addr != 0 && !is_tick_expired(entry->tick); +} + +static void ls_node_entry_update(ls_node_entry_t *entry, const int8_t ethno, const uint16_t seq, const struct ip_addr *ip_addr) +{ + entry->seq = seq; + entry->tick = ticker; + entry->ethno = ethno; + entry->ip_addr.addr = ip_addr->addr; +} + +static bool ls_node_entries_update( + ls_node_entry_t *entries, const size_t num_entries, + const int8_t ethno, const uint16_t seq, const struct ip_addr *ip_addr +) +{ + for (size_t i = 0; i < num_entries; i++) + { + if (!ls_node_entry_valid(&entries[i])) + { + ls_node_entry_update(entries+i, ethno, seq, ip_addr); + return true; + } + + if (entries[i].ip_addr.addr == ip_addr->addr && entries[i].ethno == ethno) + { + if (is_seq_newer(seq, entries[i].seq)) + { + ls_node_entry_update(entries+i, ethno, seq, ip_addr); + return true; + } + return false; + } + } + + //no space, shift the table down and take entry 0 + memmove(entries+1, entries, (num_entries-1)*sizeof(ls_node_entry_t)); + ls_node_entry_update(entries+0, ethno, seq, ip_addr); + return true; +} + +/*********************************************************************** + * storage for nodes in the network + **********************************************************************/ +static ls_node_entry_t ls_nbors[LS_NUM_NBOR_ENTRIES]; +static ls_node_entry_t ls_nodes[LS_NUM_NODE_ENTRIES]; + +/*********************************************************************** + * node table + **********************************************************************/ +static ls_node_mapping_t ls_node_maps[LS_NUM_MAP_ENTRIES]; + +const ls_node_mapping_t *link_state_route_get_node_mapping(size_t *length) +{ + *length = lengthof(ls_node_maps); + return ls_node_maps; +} + +static void add_node_mapping(const struct ip_addr *node, const struct ip_addr *nbor) +{ + //printf("add_node_mapping: %s -> %s\n", ip_addr_to_str(node), ip_addr_to_str(nbor)); + + //write into the first available slot + for (size_t i = 0; i < lengthof(ls_node_maps); i++) + { + if (ls_node_maps[i].node.addr == 0) + { + ls_node_maps[i].node.addr = node->addr; + ls_node_maps[i].nbor.addr = nbor->addr; + return; + } + } + + //otherwise, shift down the table and take slot0 + memmove(ls_node_maps+1, ls_node_maps, sizeof(ls_node_maps) - sizeof(ls_node_mapping_t)); + ls_node_maps[0].node.addr = node->addr; + ls_node_maps[0].nbor.addr = nbor->addr; +} + +static void remove_node_matches(const struct ip_addr *node) +{ + //printf("remove_node_matches: %s\n", ip_addr_to_str(node)); + + for (size_t j = 0; j < lengthof(ls_node_maps); j++) + { + //if the address is a match, clear the entry + if (ls_node_maps[j].node.addr == node->addr) + { + ls_node_maps[j].node.addr = 0; + ls_node_maps[j].nbor.addr = 0; + } + } +} + +static void update_node_mappings(const ls_data_t *ls_data) +{ + //printf("update_node_mappings: %s\n", ip_addr_to_str(&ls_data->node)); + + //remove any expired entries + for (size_t i = 0; i < lengthof(ls_nodes); i++) + { + if (ls_nodes[i].ip_addr.addr != 0 && is_tick_expired(ls_nodes[i].tick)) + { + remove_node_matches(&ls_nodes[i].ip_addr); + } + } + + //remove any matches for the current node + remove_node_matches(&ls_data->node); + + //is this a local packet? + bool is_local = false; + for (size_t e = 0; e < ethernet_ninterfaces(); e++) + { + if (ls_data->node.addr == u3_net_stack_get_ip_addr(e)->addr) is_local = true; + } + + //load entries from ls data into array + for (size_t i = 0; i < ls_data->num_nbors; i++) + { + if (is_local && i < ls_data->num_ports) continue; //ignore local ports + add_node_mapping(&ls_data->node, &ls_data->nbors[i]); + } +} + +/*********************************************************************** + * forward link state data onto all neighbors on the given port + **********************************************************************/ +static void send_link_state_data_to_all_neighbors( + const uint8_t ethno, const uint16_t seq, const ls_data_t *ls_data +){ + //exit and dont forward if the information is stale + if (!ls_node_entries_update(ls_nodes, lengthof(ls_nodes), ethno, seq, &ls_data->node)) return; + + //update the mappings with new info + update_node_mappings(ls_data); + + //forward to all neighbors + for (size_t i = 0; i < lengthof(ls_nbors); i++) + { + if (ls_nbors[i].ip_addr.addr == ls_data->node.addr) continue; //dont forward to sender + if (ls_node_entry_valid(&ls_nbors[i])) + { + if (ethernet_get_link_up(ls_nbors[i].ethno)) u3_net_stack_send_icmp_pkt( + ls_nbors[i].ethno, ICMP_IRQ, 0, + LS_ID_INFORM, seq, + &(ls_nbors[i].ip_addr), ls_data, sizeof_ls_data(ls_data) + ); + } + } + + //a change may have occured, update the cache + link_state_route_proto_update_cycle_cache(ethno); +} + +/*********************************************************************** + * handler for information reply + **********************************************************************/ +static void handle_icmp_ir( + const uint8_t ethno, + const struct ip_addr *src, const struct ip_addr *dst, + const uint16_t id, const uint16_t seq, + const void *buff, const size_t num_bytes +){ + switch (id) + { + //received a reply directly from the neighbor, add to neighbor list + case LS_ID_DISCOVER: + //printf("GOT LS_ID_DISCOVER REPLY - ID 0x%x - IP%u: %s\n", id, (int)ethno, ip_addr_to_str(u3_net_stack_get_ip_addr(ethno))); + if (ls_node_entries_update(ls_nbors, lengthof(ls_nbors), ethno, seq, src)) link_state_route_proto_flood(ethno); + break; + } +} + +/*********************************************************************** + * handler for information request + **********************************************************************/ +static void handle_icmp_irq( + const uint8_t ethno, + const struct ip_addr *src, const struct ip_addr *dst, + const uint16_t id, const uint16_t seq, + const void *buff, const size_t num_bytes +){ + switch (id) + { + //replies to discovery packets + case LS_ID_DISCOVER: + //printf("GOT LS_ID_DISCOVER REQ - IP%u: %s\n", (int)ethno, ip_addr_to_str(u3_net_stack_get_ip_addr(ethno))); + //printf("SEND LS_ID_DISCOVER REPLY - IP%u: %s\n", (int)ethno, ip_addr_to_str(u3_net_stack_get_ip_addr(ethno))); + u3_net_stack_send_icmp_pkt(ethno, ICMP_IR, 0, id, seq, src, buff, num_bytes); + break; + + //handle and forward information + case LS_ID_INFORM: + //printf("GOT LS_ID_INFORM REQ - IP%u: %s\n", (int)ethno, ip_addr_to_str(u3_net_stack_get_ip_addr(ethno))); + send_link_state_data_to_all_neighbors(ethno, seq, (const ls_data_t *)buff); + break; + }; +} + +/*********************************************************************** + * initiate a periodic update to the table + **********************************************************************/ +void link_state_route_proto_update(const uint8_t ethno) +{ + //send a discovery packet + //printf("SEND LS_ID_DISCOVER REQ - IP%u: %s\n", (int)ethno, ip_addr_to_str(u3_net_stack_get_ip_addr(ethno))); + u3_net_stack_send_icmp_pkt( + ethno, ICMP_IRQ, 0, + LS_ID_DISCOVER, current_seq++, + u3_net_stack_get_bcast(ethno), NULL, 0 + ); +} + +void link_state_route_proto_flood(const uint8_t ethno) +{ + for (size_t e = 0; e < ethernet_ninterfaces(); e++) + { + //fill link state data buffer + uint8_t buff[LS_PAYLOAD_MTU] = {}; + ls_data_t *ls_data = (ls_data_t *)buff; + ls_data->node.addr = u3_net_stack_get_ip_addr(e)->addr; + ls_data->num_nbors = 0; + ls_data->num_ports = 0; + + //first the local port links + for (size_t ej = 0; ej < ethernet_ninterfaces(); ej++) + { + if (e == ej) continue; //dont include our own port + ls_data->nbors[ls_data->num_nbors++].addr = u3_net_stack_get_ip_addr(ej)->addr; + ls_data->num_ports++; + } + + //now list the neighbors + for (size_t i = 0; i < lengthof(ls_nbors); i++) + { + if ((sizeof_ls_data(ls_data) + 4) >= LS_PAYLOAD_MTU) break; + if (ls_node_entry_valid(&ls_nbors[i]) && ls_nbors[i].ethno == e) + { + ls_data->nbors[ls_data->num_nbors++].addr = ls_nbors[i].ip_addr.addr; + } + } + + //send this data to all neighbors + send_link_state_data_to_all_neighbors(ethno, current_seq++, ls_data); + } +} + +/*********************************************************************** + * cycle detection logic + **********************************************************************/ +static void follow_links(const size_t current, struct ip_addr *nodes, bool *visited, const size_t num_nodes) +{ + if (visited[current]) return; //end the recursion + visited[current] = true; + + //follow all links where current node is the source + for (size_t i = 0; i < lengthof(ls_node_maps); i++) + { + if (ls_node_maps[i].node.addr != nodes[current].addr) continue; + + //find the index of the neighbor in the node list to recurse + for (size_t j = 0; j < num_nodes; j++) + { + if (nodes[j].addr != ls_node_maps[i].nbor.addr) continue; + follow_links(j, nodes, visited, num_nodes); + } + } +} + +bool link_state_route_proto_causes_cycle(const struct ip_addr *src, const struct ip_addr *dst) +{ + //printf("is there a cycle? %s -> %s: \n", ip_addr_to_str(src), ip_addr_to_str(dst)); + + //make a set of all nodes + size_t num_nodes = 0; + struct ip_addr nodes[LS_NUM_MAP_ENTRIES]; + for (size_t i = 0; i < lengthof(ls_node_maps); i++) + { + if (ls_node_maps[i].node.addr == 0 || ls_node_maps[i].nbor.addr == 0) continue; + //printf(" Link %s -> %s\n", ip_addr_to_str(&ls_node_maps[i].node), ip_addr_to_str(&ls_node_maps[i].nbor)); + const struct ip_addr *node = &ls_node_maps[i].node; + + //check if we have an entry + for (size_t j = 0; j < num_nodes; j++) + { + if (nodes[j].addr == node->addr) goto skip_add; + } + + //otherwise, we add the node + nodes[num_nodes++].addr = node->addr; + //printf(" Add to node set: %s\n", ip_addr_to_str(node)); + skip_add: continue; + } + + //and stateful tracking info for each node + bool visited[LS_NUM_MAP_ENTRIES]; + for (size_t i = 0; i < num_nodes; i++) visited[i] = false; + + //find our src node in the set and follow + for (size_t i = 0; i < num_nodes; i++) + { + if (nodes[i].addr == src->addr) follow_links(i, nodes, visited, num_nodes); + } + + //did we visit the destination? if so, there is a cycle + for (size_t i = 0; i < num_nodes; i++) + { + if (nodes[i].addr == dst->addr && visited[i]) + { + //printf("CAUSES CYCLE!\n"); + return true; + } + } + + //printf("no cycle found.\n"); + return false; +} + +static bool ls_causes_cycle[NETHS][NETHS]; + +void link_state_route_proto_update_cycle_cache(const uint8_t eth_src) +{ + for (size_t eth_dst = 0; eth_dst < ethernet_ninterfaces(); eth_dst++) + { + if (eth_src == eth_dst) continue; + ls_causes_cycle[eth_src][eth_dst] = link_state_route_proto_causes_cycle( + u3_net_stack_get_ip_addr(eth_src), + u3_net_stack_get_ip_addr(eth_dst) + ); + } +} + +bool link_state_route_proto_causes_cycle_cached(const uint8_t eth_src, const uint8_t eth_dst) +{ + return ls_causes_cycle[eth_src][eth_dst]; +} + +/*********************************************************************** + * init and registration code + **********************************************************************/ +void link_state_route_proto_init(void) +{ + u3_net_stack_register_icmp_handler(ICMP_IRQ, 0, &handle_icmp_irq); + u3_net_stack_register_icmp_handler(ICMP_IR, 0, &handle_icmp_ir); + + //default to causing a cycle, let the algorithm set this correctly + for (size_t i = 0; i < NETHS; i++) + { + for (size_t j = 0; j < NETHS; j++) + { + ls_causes_cycle[i][j] = true; + } + } +} diff --git a/firmware/x300/lib/mdelay.c b/firmware/x300/lib/mdelay.c new file mode 100644 index 000000000..6d2742206 --- /dev/null +++ b/firmware/x300/lib/mdelay.c @@ -0,0 +1,36 @@ +/* -*- c -*- */ +/* + * Copyright 2007 Free Software Foundation, Inc. + * Copyright 2009 Ettus Research LLC + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include "mdelay.h" +#include "wb_utils.h" +#include "printf.h" +#include <stdint.h> +//IJB FIXME. +#include "../x300/x300_defs.h" + +void mdelay(int ms){ + for(int i = 0; i < ms; i++){ + static const uint32_t num_ticks = CPU_CLOCK/1000; + const uint32_t ticks_begin = wb_peek32(SR_ADDR(RB0_BASE, RB_COUNTER)); + // printf("DEBUG: Counter is %d\n",ticks_begin); + while((wb_peek32(SR_ADDR(RB0_BASE, RB_COUNTER)) - ticks_begin) < num_ticks) { + /*NOP*/ + } + } +} diff --git a/firmware/x300/lib/print_addrs.c b/firmware/x300/lib/print_addrs.c new file mode 100644 index 000000000..6a710f75c --- /dev/null +++ b/firmware/x300/lib/print_addrs.c @@ -0,0 +1,64 @@ +// Copyright 2013 Ettus Research LLC + +#include <print_addrs.h> +#include <stddef.h> +#include <stdint.h> +#include <printf.h> + +#define MAX_MAC_CHARS 24 +#define MAX_IP_CHARS 16 + +static const char hex[16] = "0123456789ABCDEF"; + +char *mac_addr_to_str_r(const void *addr, char *str) +{ + uint8_t *p = (uint8_t *)addr; + size_t j = 0; + for(size_t i = 0; i < 6; i++) + { + if (i) str[j++] = ':'; + str[j++] = hex[(p[i] >> 4) & 0xf]; + str[j++] = hex[p[i] & 0xf]; + } + str[j++] = '\0'; + return str; +} + +char *ip_addr_to_str_r(const void *addr, char *str) +{ + uint8_t *p = (uint8_t *)addr; + sprintf(str, "%d.%d.%d.%d", p[0], p[1], p[2], p[3]); + return str; +} + +char *mac_addr_to_str(const void *addr) +{ + static size_t index = 0; + index = (index + 1) % 4; + static char str[4][MAX_MAC_CHARS]; + return mac_addr_to_str_r(addr, str[index]); +} + +char *ip_addr_to_str(const void *addr) +{ + static size_t index = 0; + index = (index + 1) % 4; + static char str[4][MAX_IP_CHARS]; + return ip_addr_to_str_r(addr, str[index]); +} + +/* +void print_mac_addr(const void *addr) +{ + char str[MAX_MAC_CHARS]; + mac_addr_to_str_r(addr, str); + printf("%s", str); +} + +void print_ip_addr(const void *addr) +{ + char str[MAX_IP_CHARS]; + ip_addr_to_str_r(addr, str); + printf("%s", str); +} +*/ diff --git a/firmware/x300/lib/printf.c b/firmware/x300/lib/printf.c new file mode 100644 index 000000000..6d6a3e712 --- /dev/null +++ b/firmware/x300/lib/printf.c @@ -0,0 +1,287 @@ +/* +File: printf.c + +Copyright (C) 2004 Kustaa Nyholm + +This library is free software; you can redistribute it and/or +modify it under the terms of the GNU Lesser General Public +License as published by the Free Software Foundation; either +version 2.1 of the License, or (at your option) any later version. + +This library is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +Lesser General Public License for more details. + +You should have received a copy of the GNU Lesser General Public +License along with this library; if not, write to the Free Software +Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +*/ + +#include "printf.h" + +typedef void (*putcf) (void*,char); +static putcf stdout_putf; +static void* stdout_putp; + +#ifdef PRINTF_LONG_LONG_SUPPORT + +static void ulli2a(unsigned long long int num, unsigned int base, int uc,char * bf) + { + int n=0; + unsigned int d=1; + while (num/d >= base) + d*=base; + while (d!=0) { + int dgt = num / d; + num%=d; + d/=base; + if (n || dgt>0|| d==0) { + *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + ++n; + } + } + *bf=0; + } + +static void lli2a (long long num, char * bf) + { + if (num<0) { + num=-num; + *bf++ = '-'; + } + ulli2a(num,10,0,bf); + } + +#endif + +#ifdef PRINTF_LONG_SUPPORT + +static void uli2a(unsigned long int num, unsigned int base, int uc,char * bf) + { + int n=0; + unsigned int d=1; + while (num/d >= base) + d*=base; + while (d!=0) { + int dgt = num / d; + num%=d; + d/=base; + if (n || dgt>0|| d==0) { + *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + ++n; + } + } + *bf=0; + } + +static void li2a (long num, char * bf) + { + if (num<0) { + num=-num; + *bf++ = '-'; + } + uli2a(num,10,0,bf); + } + +#endif + +static void ui2a(unsigned int num, unsigned int base, int uc,char * bf) + { + int n=0; + unsigned int d=1; + while (num/d >= base) + d*=base; + while (d!=0) { + int dgt = num / d; + num%= d; + d/=base; + if (n || dgt>0 || d==0) { + *bf++ = dgt+(dgt<10 ? '0' : (uc ? 'A' : 'a')-10); + ++n; + } + } + *bf=0; + } + +static void i2a (int num, char * bf) + { + if (num<0) { + num=-num; + *bf++ = '-'; + } + ui2a(num,10,0,bf); + } + +static int a2d(char ch) + { + if (ch>='0' && ch<='9') + return ch-'0'; + else if (ch>='a' && ch<='f') + return ch-'a'+10; + else if (ch>='A' && ch<='F') + return ch-'A'+10; + else return -1; + } + +static char a2i(char ch, char** src,int base,int* nump) + { + char* p= *src; + int num=0; + int digit; + while ((digit=a2d(ch))>=0) { + if (digit>base) break; + num=num*base+digit; + ch=*p++; + } + *src=p; + *nump=num; + return ch; + } + +static void putchw(void* putp,putcf putf,int n, char z, char* bf) + { + char fc=z? '0' : ' '; + char ch; + char* p=bf; + while (*p++ && n > 0) + n--; + while (n-- > 0) + putf(putp,fc); + while ((ch= *bf++)) + putf(putp,ch); + } + +void tfp_format(void* putp,putcf putf,char *fmt, va_list va) + { + char bf[12]; + + char ch; + + + while ((ch=*(fmt++))) { + if (ch!='%') + putf(putp,ch); + else { + char lz=0; +#ifdef PRINTF_LONG_SUPPORT + char lng=0; +#endif +#ifdef PRINTF_LONG_LONG_SUPPORT + char lnglng=0; +#endif + int w=0; + ch=*(fmt++); + if (ch=='0') { + ch=*(fmt++); + lz=1; + } + if (ch>='0' && ch<='9') { + ch=a2i(ch,&fmt,10,&w); + } +#ifdef PRINTF_LONG_SUPPORT + if (ch=='l') { + ch=*(fmt++); + lng=1; + } +#endif +#ifdef PRINTF_LONG_LONG_SUPPORT + if ((ch=='l')&&(lng==1)) { + ch=*(fmt++); + lnglng=1; + } +#endif + switch (ch) { + case 0: + goto abort; + case 'u' : { +#ifdef PRINTF_LONG_LONG_SUPPORT + if (lnglng) + ulli2a(va_arg(va, unsigned long long int),10,0,bf); + else +#endif +#ifdef PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int),10,0,bf); + else +#endif + ui2a(va_arg(va, unsigned int),10,0,bf); + putchw(putp,putf,w,lz,bf); + break; + } + case 'd' : { +#ifdef PRINTF_LONG_LONG_SUPPORT + if (lnglng) + lli2a(va_arg(va, long long int),bf); + else +#endif +#ifdef PRINTF_LONG_SUPPORT + if (lng) + li2a(va_arg(va, long int),bf); + else +#endif + i2a(va_arg(va, int),bf); + putchw(putp,putf,w,lz,bf); + break; + } + case 'x': case 'X' : +#ifdef PRINTF_LONG_LONG_SUPPORT + if (lnglng) + ulli2a(va_arg(va, unsigned long long int),16,(ch=='X'),bf); + else +#endif +#ifdef PRINTF_LONG_SUPPORT + if (lng) + uli2a(va_arg(va, unsigned long int),16,(ch=='X'),bf); + else +#endif + ui2a(va_arg(va, unsigned int),16,(ch=='X'),bf); + putchw(putp,putf,w,lz,bf); + break; + case 'c' : + putf(putp,(char)(va_arg(va, int))); + break; + case 's' : + putchw(putp,putf,w,0,va_arg(va, char*)); + break; + case '%' : + putf(putp,ch); + default: + break; + } + } + } + abort:; + } + + +void init_printf(void* putp,void (*putf) (void*,char)) + { + stdout_putf=putf; + stdout_putp=putp; + } + +void tfp_printf(char *fmt, ...) + { + va_list va; + va_start(va,fmt); + tfp_format(stdout_putp,stdout_putf,fmt,va); + va_end(va); + } + +static void putcp(void* p,char c) + { + *(*((char**)p))++ = c; + } + + + +void tfp_sprintf(char* s,char *fmt, ...) + { + va_list va; + va_start(va,fmt); + tfp_format(&s,putcp,fmt,va); + putcp(&s,0); + va_end(va); + } diff --git a/firmware/x300/lib/u3_net_stack.c b/firmware/x300/lib/u3_net_stack.c new file mode 100644 index 000000000..6b8ef096c --- /dev/null +++ b/firmware/x300/lib/u3_net_stack.c @@ -0,0 +1,589 @@ + +// Copyright 2012-2013 Ettus Research LLC + +#include <u3_net_stack.h> +#include <string.h> //memcmp +#include <printf.h> + +#define MAX_NETHS 4 + +typedef struct +{ + padded_eth_hdr_t eth; + struct arp_eth_ipv4 arp; +} padded_arp_t; + +typedef struct +{ + padded_eth_hdr_t eth; + struct ip_hdr ip; + struct icmp_echo_hdr icmp; +} padded_icmp_t; + +typedef struct +{ + padded_eth_hdr_t eth; + struct ip_hdr ip; + struct udp_hdr udp; +} padded_udp_t; + +/*********************************************************************** + * declares for internal handlers + **********************************************************************/ +static void handle_icmp_echo_packet( + const uint8_t ethno, + const struct ip_addr *src, const struct ip_addr *dst, + const uint16_t id, const uint16_t seq, + const void *buff, const size_t num_bytes +){ + u3_net_stack_send_icmp_pkt(ethno, ICMP_ER, 0, id, seq, src, buff, num_bytes); +} + +static void handle_icmp_dur_packet( + const uint8_t ethno, + const struct ip_addr *src, const struct ip_addr *dst, + const uint16_t id, const uint16_t seq, + const void *buff, const size_t num_bytes +); + +/*********************************************************************** + * 16-bit one's complement sum + **********************************************************************/ +static uint32_t chksum_buffer( + uint16_t *buf, size_t nshorts, + uint32_t initial_chksum +){ + uint32_t chksum = initial_chksum; + for (size_t i = 0; i < nshorts; i++) chksum += buf[i]; + + while (chksum >> 16) chksum = (chksum & 0xffff) + (chksum >> 16); + + return chksum; +} + +/*********************************************************************** + * ARP Cache implementation + **********************************************************************/ +#define ARP_CACHE_NENTRIES 32 + +static size_t arp_cache_wr_index; + +static struct ip_addr arp_cache_ips[ARP_CACHE_NENTRIES]; +static eth_mac_addr_t arp_cache_macs[ARP_CACHE_NENTRIES]; +static uint8_t arp_cache_eths[ARP_CACHE_NENTRIES]; + +void u3_net_stack_arp_cache_update(const struct ip_addr *ip_addr, const eth_mac_addr_t *mac_addr, const uint8_t ethno) +{ + for (size_t i = 0; i < ARP_CACHE_NENTRIES; i++) + { + if (memcmp(ip_addr, arp_cache_ips+i, sizeof(struct ip_addr)) == 0) + { + memcpy(arp_cache_macs+i, mac_addr, sizeof(eth_mac_addr_t)); + arp_cache_eths[i] = ethno; + return; + } + } + if (arp_cache_wr_index >= ARP_CACHE_NENTRIES) arp_cache_wr_index = 0; + memcpy(arp_cache_ips+arp_cache_wr_index, ip_addr, sizeof(struct ip_addr)); + memcpy(arp_cache_macs+arp_cache_wr_index, mac_addr, sizeof(eth_mac_addr_t)); + arp_cache_eths[arp_cache_wr_index] = ethno; + arp_cache_wr_index++; +} + +const eth_mac_addr_t *u3_net_stack_arp_cache_lookup(const struct ip_addr *ip_addr) +{ + //do a local look up on our own ports + for (size_t e = 0; e < MAX_NETHS; e++) + { + if (memcmp(ip_addr, u3_net_stack_get_ip_addr(e), sizeof(struct ip_addr)) == 0) + { + return u3_net_stack_get_mac_addr(e); + } + } + //now check the arp cache + for (size_t i = 0; i < ARP_CACHE_NENTRIES; i++) + { + if (memcmp(ip_addr, arp_cache_ips+i, sizeof(struct ip_addr)) == 0) + { + return &arp_cache_macs[i]; + } + } + return NULL; +} + +bool resolve_ip(const struct ip_addr *ip_addr, eth_mac_addr_t *mac_addr) +{ + for (size_t e = 0; e < MAX_NETHS; e++) + { + if (memcmp(u3_net_stack_get_bcast(e), ip_addr, sizeof(struct ip_addr)) == 0) + { + memset(mac_addr, 0xff, sizeof(eth_mac_addr_t)); + return true; + } + } + const eth_mac_addr_t *r = u3_net_stack_arp_cache_lookup(ip_addr); + if (r != NULL) + { + memcpy(mac_addr, r, sizeof(eth_mac_addr_t)); + return true; + } + return false; +} + +/*********************************************************************** + * Net stack config + **********************************************************************/ +static wb_pkt_iface64_config_t *pkt_iface_config = NULL; + +void u3_net_stack_init(wb_pkt_iface64_config_t *config) +{ + pkt_iface_config = config; + u3_net_stack_register_icmp_handler(ICMP_ECHO, 0, &handle_icmp_echo_packet); + u3_net_stack_register_icmp_handler(ICMP_DUR, ICMP_DUR_PORT, &handle_icmp_dur_packet); +} + +static struct ip_addr net_conf_ips[MAX_NETHS]; +static eth_mac_addr_t net_conf_macs[MAX_NETHS]; +static struct ip_addr net_conf_subnets[MAX_NETHS]; +static struct ip_addr net_conf_bcasts[MAX_NETHS]; +static uint32_t net_stat_counts[MAX_NETHS]; + +void u3_net_stack_init_eth( + const uint8_t ethno, + const eth_mac_addr_t *mac, + const struct ip_addr *ip, + const struct ip_addr *subnet +) +{ + memcpy(&net_conf_macs[ethno], mac, sizeof(eth_mac_addr_t)); + memcpy(&net_conf_ips[ethno], ip, sizeof(struct ip_addr)); + memcpy(&net_conf_subnets[ethno], subnet, sizeof(struct ip_addr)); + net_stat_counts[ethno] = 0; +} + +const struct ip_addr *u3_net_stack_get_ip_addr(const uint8_t ethno) +{ + return &net_conf_ips[ethno]; +} + +const struct ip_addr *u3_net_stack_get_subnet(const uint8_t ethno) +{ + return &net_conf_subnets[ethno]; +} + +const struct ip_addr *u3_net_stack_get_bcast(const uint8_t ethno) +{ + const uint32_t ip = u3_net_stack_get_ip_addr(ethno)->addr; + const uint32_t subnet = u3_net_stack_get_subnet(ethno)->addr; + net_conf_bcasts[ethno].addr = ip | (~subnet); + return &net_conf_bcasts[ethno]; +} + +const eth_mac_addr_t *u3_net_stack_get_mac_addr(const uint8_t ethno) +{ + return &net_conf_macs[ethno]; +} + +/*********************************************************************** + * Ethernet activity stats + **********************************************************************/ +uint32_t u3_net_stack_get_stat_counts(const uint8_t ethno) +{ + return net_stat_counts[ethno]; +} + +static void incr_stat_counts(const void *p) +{ + const padded_eth_hdr_t *eth = (const padded_eth_hdr_t *)p; + if (eth->ethno < MAX_NETHS) net_stat_counts[eth->ethno]++; +} + +/*********************************************************************** + * Ethernet handlers - send packet w/ payload + **********************************************************************/ +static void send_eth_pkt( + const void *p0, const size_t l0, + const void *p1, const size_t l1, + const void *p2, const size_t l2 +) +{ + incr_stat_counts(p0); + void *ptr = wb_pkt_iface64_tx_claim(pkt_iface_config); + size_t buff_i = 0; + + uint32_t *buff32 = (uint32_t *)ptr; + for (size_t i = 0; i < (l0+3)/4; i++) + { + buff32[buff_i++] = ((const uint32_t *)p0)[i]; + } + for (size_t i = 0; i < (l1+3)/4; i++) + { + buff32[buff_i++] = ((const uint32_t *)p1)[i]; + } + for (size_t i = 0; i < (l2+3)/4; i++) + { + buff32[buff_i++] = ((const uint32_t *)p2)[i]; + } + + // Fixes issue where we don't write an even number of 32bit words leaving last data stranded in H/W. + if ((buff_i%2) == 1) buff32[buff_i++] = 0; + + wb_pkt_iface64_tx_submit(pkt_iface_config, l0 + l1 + l2); +} + +/*********************************************************************** + * ARP handlers + **********************************************************************/ +static void send_arp_reply( + const uint8_t ethno, + const struct arp_eth_ipv4 *req, + const eth_mac_addr_t *our_mac +){ + padded_arp_t reply; + reply.eth.ethno = ethno; + memcpy(&reply.eth.dst, (eth_mac_addr_t *)req->ar_sha, sizeof(eth_mac_addr_t)); + memcpy(&reply.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t)); + reply.eth.ethertype = ETHERTYPE_ARP; + + reply.arp.ar_hrd = req->ar_hrd; + reply.arp.ar_pro = req->ar_pro; + reply.arp.ar_hln = req->ar_hln; + reply.arp.ar_pln = req->ar_pln; + reply.arp.ar_op = ARPOP_REPLY; + memcpy(reply.arp.ar_sha, our_mac, sizeof(eth_mac_addr_t)); + memcpy(reply.arp.ar_sip, req->ar_tip, sizeof(struct ip_addr)); + memcpy(reply.arp.ar_tha, req->ar_sha, sizeof(eth_mac_addr_t)); + memcpy(reply.arp.ar_tip, req->ar_sip, sizeof(struct ip_addr)); + + send_eth_pkt(&reply, sizeof(reply), NULL, 0, NULL, 0); +} + +void u3_net_stack_send_arp_request(const uint8_t ethno, const struct ip_addr *addr) +{ + padded_arp_t req; + req.eth.ethno = ethno; + memset(&req.eth.dst, 0xff, sizeof(eth_mac_addr_t)); //bcast + memcpy(&req.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t)); + req.eth.ethertype = ETHERTYPE_ARP; + + req.arp.ar_hrd = ARPHRD_ETHER; + req.arp.ar_pro = ETHERTYPE_IPV4; + req.arp.ar_hln = sizeof(eth_mac_addr_t); + req.arp.ar_pln = sizeof(struct ip_addr); + req.arp.ar_op = ARPOP_REQUEST; + memcpy(req.arp.ar_sha, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t)); + memcpy(req.arp.ar_sip, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr)); + memset(req.arp.ar_tha, 0x00, sizeof(eth_mac_addr_t)); + memcpy(req.arp.ar_tip, addr, sizeof(struct ip_addr)); + + send_eth_pkt(&req, sizeof(req), NULL, 0, NULL, 0); +} + +static void handle_arp_packet(const uint8_t ethno, const struct arp_eth_ipv4 *p) +{ + //printf("handle_arp_packet\n"); + if (p->ar_hrd != ARPHRD_ETHER + || p->ar_pro != ETHERTYPE_IPV4 + || p->ar_hln != sizeof(eth_mac_addr_t) + || p->ar_pln != sizeof(struct ip_addr)) + return; + + //got an arp reply -- injest it into the arp cache + if (p->ar_op == ARPOP_REPLY) + { + //printf("ARPOP_REPLY\n"); + struct ip_addr ip_addr; + memcpy(&ip_addr, p->ar_sip, sizeof(ip_addr)); + eth_mac_addr_t mac_addr; + memcpy(&mac_addr, p->ar_sha, sizeof(mac_addr)); + u3_net_stack_arp_cache_update(&ip_addr, &mac_addr, ethno); + } + + //got an arp request -- reply if its for our address + if (p->ar_op == ARPOP_REQUEST) + { + //printf("ARPOP_REQUEST\n"); + if (memcmp(p->ar_tip, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr)) == 0) + { + send_arp_reply(ethno, p, u3_net_stack_get_mac_addr(ethno)); + } + } +} + +/*********************************************************************** + * UDP handlers + **********************************************************************/ +#define UDP_NHANDLERS 16 + +static uint16_t udp_handler_ports[UDP_NHANDLERS]; +static u3_net_stack_udp_handler_t udp_handlers[UDP_NHANDLERS]; +static size_t udp_handlers_index = 0; + +void u3_net_stack_register_udp_handler( + const uint16_t port, + const u3_net_stack_udp_handler_t handler +) +{ + if (udp_handlers_index < UDP_NHANDLERS) + { + udp_handler_ports[udp_handlers_index] = port; + udp_handlers[udp_handlers_index] = handler; + udp_handlers_index++; + } +} + +void u3_net_stack_send_udp_pkt( + const uint8_t ethno, + const struct ip_addr *dst, + const uint16_t src_port, + const uint16_t dst_port, + const void *buff, + const size_t num_bytes +) +{ + eth_mac_addr_t dst_mac_addr; + if (!resolve_ip(dst, &dst_mac_addr)) + { + printf("u3_net_stack_send_udp_pkt arp_cache_lookup fail\n"); + return; + } + + padded_udp_t pkt; + + pkt.eth.ethno = ethno; + memcpy(&pkt.eth.dst, &dst_mac_addr, sizeof(eth_mac_addr_t)); + memcpy(&pkt.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t)); + pkt.eth.ethertype = ETHERTYPE_IPV4; + + IPH_VHLTOS_SET(&pkt.ip, 4, 5, 0); + IPH_LEN_SET(&pkt.ip, IP_HLEN + UDP_HLEN + num_bytes); + IPH_ID_SET(&pkt.ip, 0); + IPH_OFFSET_SET(&pkt.ip, IP_DF); /* don't fragment */ + IPH_TTL_SET(&pkt.ip, 32); + IPH_PROTO_SET(&pkt.ip, IP_PROTO_UDP); + IPH_CHKSUM_SET(&pkt.ip, 0); + memcpy(&pkt.ip.src, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr)); + memcpy(&pkt.ip.dest, dst, sizeof(struct ip_addr)); + + IPH_CHKSUM_SET(&pkt.ip, ~chksum_buffer( + (unsigned short *) &pkt.ip, sizeof(pkt.ip)/sizeof(short), 0 + )); + + pkt.udp.src = src_port; + pkt.udp.dest = dst_port; + pkt.udp.len = UDP_HLEN + num_bytes; + pkt.udp.chksum = 0; + + send_eth_pkt(&pkt, sizeof(pkt), buff, num_bytes, NULL, 0); +} + +static void handle_udp_packet( + const uint8_t ethno, + const struct ip_addr *src, + const struct ip_addr *dst, + const struct udp_hdr *udp, + const size_t num_bytes +){ + for (size_t i = 0; i < udp_handlers_index; i++) + { + if (udp_handler_ports[i] == udp->dest) + { + udp_handlers[i]( + ethno, src, u3_net_stack_get_ip_addr(ethno), udp->src, udp->dest, + ((const uint8_t *)udp) + sizeof(struct udp_hdr), + num_bytes - UDP_HLEN + ); + return; + } + } + printf("Unhandled UDP packet src=%u, dest=%u\n", udp->src, udp->dest); + //TODO send destination unreachable +} + +/*********************************************************************** + * ICMP handlers + **********************************************************************/ +#define ICMP_NHANDLERS 8 + +static uint8_t icmp_handler_types[ICMP_NHANDLERS]; +static uint8_t icmp_handler_codes[ICMP_NHANDLERS]; +static u3_net_stack_icmp_handler_t icmp_handlers[ICMP_NHANDLERS]; +static size_t icmp_handlers_index = 0; + +void u3_net_stack_register_icmp_handler( + const uint8_t type, + const uint8_t code, + const u3_net_stack_icmp_handler_t handler +) +{ + if (icmp_handlers_index < ICMP_NHANDLERS) + { + icmp_handler_types[icmp_handlers_index] = type; + icmp_handler_codes[icmp_handlers_index] = code; + icmp_handlers[icmp_handlers_index] = handler; + icmp_handlers_index++; + } +} + +static void handle_icmp_packet( + const uint8_t ethno, + const struct ip_addr *src, + const struct ip_addr *dst, + const struct icmp_echo_hdr *icmp, + const size_t num_bytes +){ + + for (size_t i = 0; i < icmp_handlers_index; i++) + { + if (icmp_handler_types[i] == icmp->type && icmp_handler_codes[i] == icmp->code) + { + icmp_handlers[i]( + ethno, src, u3_net_stack_get_ip_addr(ethno), icmp->id, icmp->seqno, + ((const uint8_t *)icmp) + sizeof(struct icmp_echo_hdr), + num_bytes - sizeof(struct icmp_echo_hdr) + ); + return; + } + } + printf("Unhandled ICMP packet type=%u\n", icmp->type); +} + +static void handle_icmp_dur_packet( + const uint8_t ethno, + const struct ip_addr *src, const struct ip_addr *dst, + const uint16_t id, const uint16_t seq, + const void *buff, const size_t num_bytes +){ + struct ip_hdr *ip = (struct ip_hdr *)buff; + struct udp_hdr *udp = (struct udp_hdr *)(((char *)ip) + IP_HLEN); + if (IPH_PROTO(ip) != IP_PROTO_UDP) return; + for (size_t i = 0; i < udp_handlers_index; i++) + { + if (udp_handler_ports[i] == udp->src) + { + udp_handlers[i](ethno, + src, u3_net_stack_get_ip_addr(ethno), + udp->src, udp->dest, NULL, 0 + ); + return; + } + } +} + +void u3_net_stack_send_icmp_pkt( + const uint8_t ethno, + const uint8_t type, + const uint8_t code, + const uint16_t id, + const uint16_t seq, + const struct ip_addr *dst, + const void *buff, + const size_t num_bytes +) +{ + eth_mac_addr_t dst_mac_addr; + if (!resolve_ip(dst, &dst_mac_addr)) + { + printf("u3_net_stack_send_echo_request arp_cache_lookup fail\n"); + return; + } + + padded_icmp_t pkt; + + pkt.eth.ethno = ethno; + memcpy(&pkt.eth.dst, &dst_mac_addr, sizeof(eth_mac_addr_t)); + memcpy(&pkt.eth.src, u3_net_stack_get_mac_addr(ethno), sizeof(eth_mac_addr_t)); + pkt.eth.ethertype = ETHERTYPE_IPV4; + + IPH_VHLTOS_SET(&pkt.ip, 4, 5, 0); + IPH_LEN_SET(&pkt.ip, IP_HLEN + sizeof(pkt.icmp) + num_bytes); + IPH_ID_SET(&pkt.ip, 0); + IPH_OFFSET_SET(&pkt.ip, IP_DF); /* don't fragment */ + IPH_TTL_SET(&pkt.ip, 32); + IPH_PROTO_SET(&pkt.ip, IP_PROTO_ICMP); + IPH_CHKSUM_SET(&pkt.ip, 0); + memcpy(&pkt.ip.src, u3_net_stack_get_ip_addr(ethno), sizeof(struct ip_addr)); + memcpy(&pkt.ip.dest, dst, sizeof(struct ip_addr)); + + IPH_CHKSUM_SET(&pkt.ip, ~chksum_buffer( + (unsigned short *) &pkt.ip, sizeof(pkt.ip)/sizeof(short), 0 + )); + + pkt.icmp.type = type; + pkt.icmp.code = code; + pkt.icmp.chksum = 0; + pkt.icmp.id = id; + pkt.icmp.seqno = seq; + pkt.icmp.chksum = ~chksum_buffer( //data checksum + (unsigned short *)buff, + num_bytes/sizeof(short), + chksum_buffer( //header checksum + (unsigned short *)&pkt.icmp, + sizeof(pkt.icmp)/sizeof(short), + 0) + ); + + send_eth_pkt(&pkt, sizeof(pkt), buff, num_bytes, NULL, 0); +} + +/*********************************************************************** + * Ethernet handlers + **********************************************************************/ +static void handle_eth_packet(const void *buff, const size_t num_bytes) +{ + const padded_eth_hdr_t *eth_hdr = (padded_eth_hdr_t *)buff; + const uint8_t *eth_body = ((const uint8_t *)buff) + sizeof(padded_eth_hdr_t); + //printf("handle_eth_packet got ethertype 0x%x\n", (unsigned)eth_hdr->ethertype); + + if (eth_hdr->ethertype == ETHERTYPE_ARP) + { + //printf("eth_hdr->ethertype == ETHERTYPE_ARP\n"); + const struct arp_eth_ipv4 *arp = (const struct arp_eth_ipv4 *)eth_body; + handle_arp_packet(eth_hdr->ethno, arp); + } + else if (eth_hdr->ethertype == ETHERTYPE_IPV4) + { + //printf("eth_hdr->ethertype == ETHERTYPE_IPV4\n"); + const struct ip_hdr *ip = (const struct ip_hdr *)eth_body; + const uint8_t *ip_body = eth_body + IP_HLEN; + + if (IPH_V(ip) != 4 || IPH_HL(ip) != 5) return;// ignore pkts w/ bad version or options + if (IPH_OFFSET(ip) & (IP_MF | IP_OFFMASK)) return;// ignore fragmented packets + + //TODO -- only handle when the mac is bcast or IP is for us + + u3_net_stack_arp_cache_update(&ip->src, ð_hdr->src, eth_hdr->ethno); + + if (IPH_PROTO(ip) == IP_PROTO_UDP) + { + handle_udp_packet( + eth_hdr->ethno, &ip->src, &ip->dest, + (const struct udp_hdr *)ip_body, + IPH_LEN(ip) - IP_HLEN + ); + } + + if (IPH_PROTO(ip) == IP_PROTO_ICMP) + { + handle_icmp_packet( + eth_hdr->ethno, &ip->src, &ip->dest, + (const struct icmp_echo_hdr *)ip_body, + IPH_LEN(ip) - IP_HLEN + ); + } + } + else return; // Not ARP or IPV4, ignore +} + +void u3_net_stack_handle_one(void) +{ + size_t num_bytes = 0; + const void *ptr = wb_pkt_iface64_rx_try_claim(pkt_iface_config, &num_bytes); + if (ptr != NULL) + { + //printf("u3_net_stack_handle_one got %u bytes\n", (unsigned)num_bytes); + incr_stat_counts(ptr); + handle_eth_packet(ptr, num_bytes); + wb_pkt_iface64_rx_release(pkt_iface_config); + } +} diff --git a/firmware/x300/lib/udp_uart.c b/firmware/x300/lib/udp_uart.c new file mode 100644 index 000000000..d078aa2c3 --- /dev/null +++ b/firmware/x300/lib/udp_uart.c @@ -0,0 +1,149 @@ +/* + * Copyright 2011-2013 Ettus Research LLC + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#include <wb_uart.h> +#include <udp_uart.h> +#include <u3_net_stack.h> + +/*********************************************************************** + * Constants + **********************************************************************/ +#define MAX_NUM_UARTS 4 + +static const size_t num_idle_cyc_b4_flush = 22; + +/*********************************************************************** + * Globals + **********************************************************************/ +typedef struct +{ + uint32_t uart_base; + struct ip_addr host_addr; + int host_ethno; + uint16_t host_port; + uint16_t local_port; + __attribute__ ((aligned (16))) uint8_t buf[256]; + size_t len; //length of buffer + size_t cyc; //idle cycle count +} udp_uart_state_t; + +static udp_uart_state_t _states[MAX_NUM_UARTS]; + +static int udp_uart_lookup(const uint16_t port) +{ + for (size_t i = 0; i < MAX_NUM_UARTS; i++) + { + if (_states[i].local_port == port) return i; + } + return -1; +} + +/*********************************************************************** + * UDP handler for UARTs + **********************************************************************/ +static void handle_uart_data_packet( + const uint8_t ethno, + const struct ip_addr *src, const struct ip_addr *dst, + const uint16_t src_port, const uint16_t dst_port, + const void *buff, const size_t num_bytes +){ + //handle ICMP destination unreachable + if (buff == NULL) + { + const size_t which = udp_uart_lookup(src_port); + if (which == -1) return; + _states[which].host_port = 0; + } + + //handle a regular blocking UART write + else + { + const size_t which = udp_uart_lookup(dst_port); + if (which == -1) return; + _states[which].host_ethno = ethno; + _states[which].host_addr = *src; + _states[which].host_port = src_port; + for (size_t i = 0; i < num_bytes; i++) + { + const char ch = ((const char *)buff)[i]; + if (ch == '\n') wb_uart_putc(_states[which].uart_base, (int)'\r'); + wb_uart_putc(_states[which].uart_base, (int)ch); + udp_uart_poll(); + } + } +} + +/*********************************************************************** + * Public init function + **********************************************************************/ +void udp_uart_init(const uint32_t uart_base, const uint16_t udp_port) +{ + for (size_t i = 0; i < MAX_NUM_UARTS; i++) + { + if (_states[i].uart_base != 0) continue; + _states[i].uart_base = uart_base; + _states[i].local_port = udp_port; + _states[i].host_port = 0; //reset to null port + _states[i].len = 0; + _states[i].cyc = 0; + u3_net_stack_register_udp_handler(udp_port, &handle_uart_data_packet); + return; + } +} + +/*********************************************************************** + * Public poll function + **********************************************************************/ +void udp_uart_poll(void) +{ + for (size_t i = 0; i < MAX_NUM_UARTS; i++) + { + if (_states[i].uart_base == 0) continue; + + bool newline = false; + udp_uart_state_t *state = &_states[i]; + + //read all characters we can without blocking + for (size_t j = state->len; j < sizeof(state->buf); j++) + { + int ret = wb_uart_getc(state->uart_base); + if (ret == -1) break; + char ch = (char) ret; + if (ch == '\n' || ch == '\r') newline = true; + state->buf[j] = ch; + state->len++; + state->cyc = 0; //reset idle cycles + } + + //nothing in buffer, continue to next uart + if (state->len == 0) continue; + + //send out a message if newline or forced flush + if (newline || state->cyc++ > num_idle_cyc_b4_flush) + { + if (state->host_port != 0) u3_net_stack_send_udp_pkt( + state->host_ethno, + &state->host_addr, + state->local_port, + state->host_port, + state->buf, state->len + ); + state->len = 0; + state->cyc = 0; + } + } +} diff --git a/firmware/x300/lib/wb_i2c.c b/firmware/x300/lib/wb_i2c.c new file mode 100644 index 000000000..611908e18 --- /dev/null +++ b/firmware/x300/lib/wb_i2c.c @@ -0,0 +1,137 @@ + +// Copyright 2012 Ettus Research LLC +/* + * Copyright 2007 Free Software Foundation, Inc. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * 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. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +// NOTE: this driver left shifts a "7bit" I2C address by one bit and puts a R/W bit as bit 0. +// Some devices may specify there I2C address assuming this R/W bit is already in place. + +#include <wb_i2c.h> + +typedef struct { + volatile uint32_t prescaler_lo; // r/w + volatile uint32_t prescaler_hi; // r/w + volatile uint32_t ctrl; // r/w + volatile uint32_t data; // wr = transmit reg; rd = receive reg + volatile uint32_t cmd_status; // wr = command reg; rd = status reg +} i2c_regs_t; + +#define i2c_regs ((i2c_regs_t *) base) + +#define I2C_CTRL_EN (1 << 7) // core enable +#define I2C_CTRL_IE (1 << 6) // interrupt enable + +// +// STA, STO, RD, WR, and IACK bits are cleared automatically +// +#define I2C_CMD_START (1 << 7) // generate (repeated) start condition +#define I2C_CMD_STOP (1 << 6) // generate stop condition +#define I2C_CMD_RD (1 << 5) // read from slave +#define I2C_CMD_WR (1 << 4) // write to slave +#define I2C_CMD_NACK (1 << 3) // when a rcvr, send ACK (ACK=0) or NACK (ACK=1) +#define I2C_CMD_RSVD_2 (1 << 2) // reserved +#define I2C_CMD_RSVD_1 (1 << 1) // reserved +#define I2C_CMD_IACK (1 << 0) // set to clear pending interrupt + +#define I2C_ST_RXACK (1 << 7) // Received acknowledgement from slave (1 = NAK, 0 = ACK) +#define I2C_ST_BUSY (1 << 6) // 1 after START signal detected; 0 after STOP signal detected +#define I2C_ST_AL (1 << 5) // Arbitration lost. 1 when core lost arbitration +#define I2C_ST_RSVD_4 (1 << 4) // reserved +#define I2C_ST_RSVD_3 (1 << 3) // reserved +#define I2C_ST_RSVD_2 (1 << 2) // reserved +#define I2C_ST_TIP (1 << 1) // Transfer-in-progress +#define I2C_ST_IP (1 << 0) // Interrupt pending + +void wb_i2c_init(const uint32_t base, const size_t clk_rate) +{ + // prescaler divisor values for 100 kHz I2C [uses 5 * SCLK internally] + const uint16_t prescaler = (clk_rate/(5 * 400000)) - 1; + i2c_regs->prescaler_lo = prescaler & 0xff; + i2c_regs->prescaler_hi = (prescaler >> 8) & 0xff; + + i2c_regs->ctrl = I2C_CTRL_EN; //| I2C_CTRL_IE; // enable core +} + +static inline void +wait_for_xfer(const uint32_t base) +{ + while (i2c_regs->cmd_status & I2C_ST_TIP) // wait for xfer to complete + ; +} + +static inline bool +wait_chk_ack(const uint32_t base) +{ + wait_for_xfer(base); + + if ((i2c_regs->cmd_status & I2C_ST_RXACK) != 0){ // target NAK'd + return false; + } + return true; +} + +bool wb_i2c_read(const uint32_t base, const uint8_t i2c_addr, uint8_t *buf, size_t len) +{ + if (len == 0) // reading zero bytes always works + return true; + + while (i2c_regs->cmd_status & I2C_ST_BUSY) + ; + + i2c_regs->data = (i2c_addr << 1) | 1; // 7 bit address and read bit (1) + // generate START and write addr + i2c_regs->cmd_status = I2C_CMD_WR | I2C_CMD_START; + if (!wait_chk_ack(base)) + goto fail; + + for (; len > 0; buf++, len--){ + i2c_regs->cmd_status = I2C_CMD_RD | (len == 1 ? (I2C_CMD_NACK | I2C_CMD_STOP) : 0); + wait_for_xfer(base); + *buf = i2c_regs->data; + } + return true; + + fail: + i2c_regs->cmd_status = I2C_CMD_STOP; // generate STOP + return false; +} + +bool wb_i2c_write(const uint32_t base, const uint8_t i2c_addr, const uint8_t *buf, size_t len) +{ + while (i2c_regs->cmd_status & I2C_ST_BUSY) + ; + + i2c_regs->data = (i2c_addr << 1) | 0; // 7 bit address and write bit (0) + + // generate START and write addr (and maybe STOP) + i2c_regs->cmd_status = I2C_CMD_WR | I2C_CMD_START | (len == 0 ? I2C_CMD_STOP : 0); + if (!wait_chk_ack(base)) + goto fail; + + for (; len > 0; buf++, len--){ + i2c_regs->data = *buf; + i2c_regs->cmd_status = I2C_CMD_WR | (len == 1 ? I2C_CMD_STOP : 0); + if (!wait_chk_ack(base)) + goto fail; + } + return true; + + fail: + i2c_regs->cmd_status = I2C_CMD_STOP; // generate STOP + return false; +} + diff --git a/firmware/x300/lib/wb_pkt_iface64.c b/firmware/x300/lib/wb_pkt_iface64.c new file mode 100644 index 000000000..3c0774bdd --- /dev/null +++ b/firmware/x300/lib/wb_pkt_iface64.c @@ -0,0 +1,87 @@ +// Copyright 2012 Ettus Research LLC + +#include <wb_pkt_iface64.h> +#include <wb_utils.h> +#include <printf.h> + +#define NUM_BYTES_MASK 0x1fff + +static uint32_t get_status(wb_pkt_iface64_config_t *config) +{ + return wb_peek32(config->config_addr); +} + +static void set_control(wb_pkt_iface64_config_t *config) +{ + wb_poke32(config->config_addr, config->ctrl); +} + +wb_pkt_iface64_config_t wb_pkt_iface64_init(const uint32_t base, const size_t ctrl_offset) +{ + wb_pkt_iface64_config_t config; + config.base = base; + config.ctrl = 0; + config.config_addr = base + ctrl_offset; + set_control(&config); + wb_pkt_iface64_rx_release(&config); //always release, in case left in a filled state + return config; +} + +const void *wb_pkt_iface64_rx_try_claim(wb_pkt_iface64_config_t *config, size_t *num_bytes) +{ + const uint32_t status = get_status(config); + const uint32_t rx_state_flag = (status >> 31) & 0x1; + *num_bytes = (status & NUM_BYTES_MASK); + //if (*num_bytes & 0x7) *num_bytes -= 8; //adjust for tuser + if (rx_state_flag == 0) return NULL; + return (void *)config->base; +} + +void wb_pkt_iface64_rx_release(wb_pkt_iface64_config_t *config) +{ + config->ctrl |= 1ul << 31; //does a release + set_control(config); + while (true) + { + const uint32_t status = get_status(config); + const uint32_t rx_state_flag = (status >> 31) & 0x1; + if (rx_state_flag == 0) + { + config->ctrl &= ~(1ul << 31); //allows for next claim + set_control(config); + return; + } + } +} + +void *wb_pkt_iface64_tx_claim(wb_pkt_iface64_config_t *config) +{ + while (true) + { + const uint32_t status = get_status(config); + const uint32_t tx_state_flag = (status >> 30) & 0x1; + if (tx_state_flag == 1) break; + } + return (void *)config->base; +} + +void wb_pkt_iface64_tx_submit(wb_pkt_iface64_config_t *config, size_t num_bytes) +{ + config->ctrl |= (1ul << 30); //allows for next claim + config->ctrl &= ~(NUM_BYTES_MASK); //clear num bytes + if (num_bytes & 0x7) num_bytes += 8; //adjust for tuser + config->ctrl |= num_bytes & NUM_BYTES_MASK; //set num bytes + set_control(config); + + //wait for state machine to transition + while (true) + { + const uint32_t status = get_status(config); + const uint32_t tx_state_flag = (status >> 30) & 0x1; + if (tx_state_flag == 0) break; + } + + config->ctrl &= ~(1ul << 30); //release + set_control(config); + +} diff --git a/firmware/x300/lib/wb_uart.c b/firmware/x300/lib/wb_uart.c new file mode 100644 index 000000000..368d0e150 --- /dev/null +++ b/firmware/x300/lib/wb_uart.c @@ -0,0 +1,34 @@ +// Copyright 2012 Ettus Research LLC + +#include <wb_uart.h> +#include <wb_utils.h> + +localparam SUART_CLKDIV = 0; +localparam SUART_TXLEVEL = 1; +localparam SUART_RXLEVEL = 2; +localparam SUART_TXCHAR = 3; +localparam SUART_RXCHAR = 4; + +void wb_uart_init(const uint32_t base, const size_t div) +{ + wb_poke32(base + SUART_CLKDIV*4, div); +} + +void wb_uart_putc(const uint32_t base, const int ch) +{ + while (wb_peek32(base + SUART_TXLEVEL*4) == 0); + wb_poke32(base + SUART_TXCHAR*4, ch); +} + +bool wb_uart_try_putc(const uint32_t base, const int ch) +{ + if (wb_peek32(base + SUART_TXLEVEL*4) == 0) return false; + wb_poke32(base + SUART_TXCHAR*4, ch); + return true; +} + +int wb_uart_getc(const uint32_t base) +{ + if (wb_peek32(base + SUART_RXLEVEL*4) == 0) return -1; + return wb_peek32(base + SUART_RXCHAR*4); +} |