diff --git a/src/hal/drivers/mesa-hostmot2/hm2_eth.c b/src/hal/drivers/mesa-hostmot2/hm2_eth.c index 4c671e76092..39a36c6b5af 100644 --- a/src/hal/drivers/mesa-hostmot2/hm2_eth.c +++ b/src/hal/drivers/mesa-hostmot2/hm2_eth.c @@ -71,7 +71,7 @@ static int *kvlist_lookup(struct rtapi_list_head *head, const char *name) { struct kvlist *ent = rtapi_list_entry(ptr, struct kvlist, list); if(strncmp(name, ent->key, sizeof(ent->key)) == 0) return &ent->value; } - struct kvlist *ent = rtapi_kzalloc(sizeof(struct kvlist), RTAPI_GPF_KERNEL); + struct kvlist *ent = rtapi_kzalloc(sizeof(struct kvlist), RTAPI_GFP_KERNEL); if(!ent) return NULL; strncpy(ent->key, name, sizeof(ent->key)-1); @@ -680,7 +680,7 @@ static char* inet_ntoa_buf(struct in_addr in, char *buf, size_t n) { } char* fetch_ifname(int sockfd, char *buf, size_t n) { - struct sockaddr_in srcaddr; + struct sockaddr_in srcaddr = {0}; struct ifaddrs *ifa, *it; socklen_t addrlen = sizeof(srcaddr); @@ -707,7 +707,7 @@ char* fetch_ifname(int sockfd, char *buf, size_t n) { } static int install_firewall_board(int sockfd) { - struct sockaddr_in srcaddr, dstaddr; + struct sockaddr_in srcaddr = {0}, dstaddr = {0}; char srchost[16], dsthost[16]; // enough for 255.255.255.255\0 char dport_s[8], sport_s[8]; @@ -800,11 +800,11 @@ int fetch_hwaddr(hm2_eth_t *board, unsigned char buf[6]) { unsigned char response[6]; LBP16_INIT_PACKET4(packet, 0x4983, 0x0002); int res = eth_socket_send(board, &packet, sizeof(packet)); - if(res < 0) return -errno; + if(res != (int)sizeof(packet)) return res < 0 ? -errno : -EIO; int i=0; res = eth_socket_recv(board, &response, sizeof(response), RECV_TIMEOUT_NON_RT_NS); - if(res < 0) return -errno; + if(res != (int)sizeof(response)) return res < 0 ? -errno : -EIO; // eeprom order is backwards from arp AF_LOCAL order for(i=0; i<6; i++) buf[i] = response[5-i]; @@ -877,14 +877,24 @@ static inline int eth_socket_recv(hm2_eth_t *board, void *buffer, int len, int r /// hm2_eth io functions +static bool hm2_eth_valid_transfer_size(int size) { + return size > 0 + && (size % sizeof(rtapi_u32)) == 0 + && size / (int)sizeof(rtapi_u32) <= LBP16_MAX_PACKET_DATA_SIZE; +} + static int hm2_eth_read(hm2_lowlevel_io_t *this, rtapi_u32 addr, void *buffer, int size) { hm2_eth_t *board = this->private; int send, recv; - rtapi_u8 tmp_buffer[size + 4]; + rtapi_u8 tmp_buffer[LBP16_MAX_PACKET_DATA_SIZE * sizeof(rtapi_u32)]; long long t1, t2; if (comm_active == 0) return 1; if (size == 0) return 1; + if (!hm2_eth_valid_transfer_size(size)) { + THIS_ERR("hm2_eth_read: invalid transfer size %d\n", size); + return 0; + } board->read_cnt++; if(rtapi_task_self() >= 0) { @@ -901,8 +911,12 @@ static int hm2_eth_read(hm2_lowlevel_io_t *this, rtapi_u32 addr, void *buffer, i LBP16_INIT_PACKET4(read_packet, CMD_READ_HOSTMOT2_ADDR32_INCR(size/4), addr & 0xFFFF); send = eth_socket_send(board, (void*) &read_packet, sizeof(read_packet)); - if(send < 0) - LL_PRINT("ERROR: sending packet: %s\n", strerror(errno)); + if(send != (int)sizeof(read_packet)) { + LL_PRINT("ERROR: sending read packet%s%s\n", + send < 0 ? ": " : "", + send < 0 ? strerror(errno) : ""); + return 0; + } LL_PRINT_IF(debug, "read(%d) : PACKET SENT [CMD:%02X%02X | ADDR: %02X%02X | SIZE: %d]\n", board->read_cnt, read_packet.cmd_hi, read_packet.cmd_lo, read_packet.addr_lo, read_packet.addr_hi, size); @@ -915,7 +929,7 @@ static int hm2_eth_read(hm2_lowlevel_io_t *this, rtapi_u32 addr, void *buffer, i } else { LL_PRINT_IF(debug, "read(%d) : PACKET RECV [SIZE: %d | TIME: %lld]\n", board->read_cnt, recv, t2 - t1); } - if (recv < 0) + if (recv != size) return 0; memcpy(buffer, tmp_buffer, size); return 1; // success @@ -925,12 +939,18 @@ static void hm2_eth_reset_queued_reads(hm2_eth_t *board){ board->read_packet_ptr = board->read_packet; board->queue_reads_count = 0; board->queue_buff_size = 0; + board->read_queue_error = false; } static int hm2_eth_send_queued_reads(hm2_lowlevel_io_t *this) { hm2_eth_t *board = this->private; int send; + if (board->read_queue_error) { + hm2_eth_reset_queued_reads(board); + return 0; + } + //Check size we are going to write size_t read_packet_size = board->read_packet_ptr - board->read_packet; if (read_packet_size + 3*sizeof(lbp16_cmd_addr) + sizeof(board->read_cnt) > sizeof(board->read_packet)) { @@ -973,9 +993,13 @@ static int hm2_eth_send_queued_reads(hm2_lowlevel_io_t *this) { board->queue_reads_count++; board->queue_buff_size += sizeof(board->confirm_rw_cnt); - send = eth_socket_send(board, (void*) &board->read_packet, board->read_packet_ptr - board->read_packet); - if(send < 0) { - LL_PRINT("ERROR: sending packet: %s\n", strerror(errno)); + size_t packet_size = board->read_packet_ptr - board->read_packet; + send = eth_socket_send(board, (void*) &board->read_packet, packet_size); + if(send != (int)packet_size) { + LL_PRINT("ERROR: sending queued-read packet%s%s\n", + send < 0 ? ": " : "", + send < 0 ? strerror(errno) : ""); + hm2_eth_reset_queued_reads(board); return 0; } return 1; @@ -1011,8 +1035,9 @@ static void decrement_soft_error(hm2_eth_t *board) { static int hm2_eth_receive_queued_reads(hm2_lowlevel_io_t *this) { hm2_eth_t *board = this->private; + bool write_batch_dropped = board->write_batch_dropped; + board->write_batch_dropped = false; // This receive accounts for the failed cycle. int recv, i = 0; - rtapi_u8 tmp_buffer[board->queue_buff_size]; unsigned long long t1, t2; t1 = rtapi_get_time(); @@ -1024,6 +1049,13 @@ static int hm2_eth_receive_queued_reads(hm2_lowlevel_io_t *this) { board->comm_error_counter = 0; } + // A failed send clears the queue, but HostMot2 still calls receive. + if(board->queue_buff_size == 0) { + if(!record_soft_error(board)) return 0; + return -EAGAIN; + } + rtapi_u8 tmp_buffer[board->queue_buff_size]; + long read_timeout = board->hal ? hal_get_si32(board->hal->read_timeout) : 1600000; if(read_timeout <= 0)//less than or equal to 0, use 80% of the thread period. read_timeout = 80; @@ -1069,7 +1101,8 @@ static int hm2_eth_receive_queued_reads(hm2_lowlevel_io_t *this) { // The has_written_cnt is necessary because the thread cycle uses first // read, then write. If we didn't check, then we'd be using uninitialized // data read back from the board. - if(board->has_written_cnt && board->write_cnt != board->confirm_rw_cnt.write_cnt) { + if(write_batch_dropped || (board->has_written_cnt && board->write_cnt != board->confirm_rw_cnt.write_cnt)) { + if(write_batch_dropped && !board->hal) return 0; result = record_soft_error(board); } else { decrement_soft_error(board); @@ -1085,8 +1118,11 @@ static int hm2_eth_reset(hm2_lowlevel_io_t *this) { lbp16_cmd_addr_data32 bite_packet; LBP16_INIT_PACKET8(bite_packet, CMD_WRITE_HOSTMOT2_ADDR32_INCR(1), HM2_ADDR_WATCHDOG, 0x0001); int ret = eth_socket_send(board, (void*) &bite_packet, sizeof(bite_packet)); - if(ret < 0) perror("eth_socket_send(bite_packet)"); - return ret < 0 ? -errno : 0; + if(ret != (int)sizeof(bite_packet)) { + if (ret < 0) perror("eth_socket_send(bite_packet)"); + return ret < 0 ? -errno : -EIO; + } + return 0; } static int hm2_eth_enqueue_read(hm2_lowlevel_io_t *this, rtapi_u32 addr, void *buffer, int size) { @@ -1094,15 +1130,30 @@ static int hm2_eth_enqueue_read(hm2_lowlevel_io_t *this, rtapi_u32 addr, void *b if (comm_active == 0) return 1; if (size == 0) return 1; - //Check size we are going to write + if (board->read_queue_error) return 0; + if (!hm2_eth_valid_transfer_size(size)) { + THIS_ERR("hm2_eth_enqueue_read: invalid transfer size %d\n", size); + goto fail; + } + // hm2_eth_send_queued_reads() appends two internal confirmation reads. + if (board->queue_reads_count >= MAX_ETH_READS - 2) { + THIS_ERR("hm2_eth_enqueue_read: read queue full (%d)\n", board->queue_reads_count); + goto fail; + } size_t read_packet_size = board->read_packet_ptr - board->read_packet; - if (read_packet_size + sizeof(lbp16_cmd_addr) > sizeof(board->read_packet)) { - LL_PRINT("ERROR: enqueue_read: buffer full\n"); - return 0; + size_t read_packet_trailer_size = + 3 * sizeof(lbp16_cmd_addr) + sizeof(board->read_cnt); + if (read_packet_size + sizeof(lbp16_cmd_addr) + + read_packet_trailer_size > sizeof(board->read_packet)) { + THIS_ERR("hm2_eth_enqueue_read: read packet buffer size exceeded\n"); + goto fail; } - if (board->queue_reads_count + 1 > MAX_ETH_READS) { - LL_PRINT("ERROR: enqueue_read: queue_reads full\n"); - return 0; + size_t response_trailer_size = + sizeof(board->rxudpcount) + sizeof(board->confirm_rw_cnt); + if ((size_t)board->queue_buff_size + size + response_trailer_size + > HM2_ETH_PACKET_SIZE) { + THIS_ERR("hm2_eth_enqueue_read: response packet size exceeded\n"); + goto fail; } LBP16_INIT_PACKET4(*(lbp16_cmd_addr*)board->read_packet_ptr, CMD_READ_HOSTMOT2_ADDR32_INCR(size/4), addr); @@ -1113,6 +1164,10 @@ static int hm2_eth_enqueue_read(hm2_lowlevel_io_t *this, rtapi_u32 addr, void *b board->queue_reads_count++; board->queue_buff_size += size; return 1; + +fail: + board->read_queue_error = true; + return 0; } static int hm2_eth_enqueue_write(hm2_lowlevel_io_t *this, rtapi_u32 addr, const void *buffer, int size); @@ -1129,6 +1184,10 @@ static int hm2_eth_write(hm2_lowlevel_io_t *this, rtapi_u32 addr, const void *bu if (comm_active == 0) return 1; if (size == 0) return 1; + if (!hm2_eth_valid_transfer_size(size)) { + THIS_ERR("hm2_eth_write: invalid transfer size %d\n", size); + return 0; + } hm2_eth_t *board = this->private; board->write_cnt++; @@ -1136,8 +1195,12 @@ static int hm2_eth_write(hm2_lowlevel_io_t *this, rtapi_u32 addr, const void *bu LBP16_INIT_PACKET4(packet.wr_packet, CMD_WRITE_HOSTMOT2_ADDR32_INCR(size/4), addr & 0xFFFF); send = eth_socket_send(board, (void*) &packet, sizeof(lbp16_cmd_addr) + size); - if(send < 0) - LL_PRINT("ERROR: sending packet: %s\n", strerror(errno)); + if(send != (int)(sizeof(lbp16_cmd_addr) + size)) { + LL_PRINT("ERROR: sending write packet%s%s\n", + send < 0 ? ": " : "", + send < 0 ? strerror(errno) : ""); + return 0; + } LL_PRINT_IF(debug, "write(%d): PACKET SENT [CMD:%02X%02X | ADDR: %02X%02X | SIZE: %d]\n", board->write_cnt, packet.wr_packet.cmd_hi, packet.wr_packet.cmd_lo, packet.wr_packet.addr_lo, packet.wr_packet.addr_hi, size); @@ -1151,10 +1214,12 @@ static int hm2_eth_send_queued_writes(hm2_lowlevel_io_t *this) { //Check size we are going to write size_t write_packet_size = board->write_packet_ptr - board->write_packet; - if (write_packet_size + sizeof(lbp16_cmd_addr) + sizeof(board->write_cnt) > sizeof(board->write_packet)) { - LL_PRINT("ERROR: send_queued_writes: buffer full, dropping all data\n"); + if (board->write_queue_error || write_packet_size + sizeof(lbp16_cmd_addr) + sizeof(board->write_cnt) > sizeof(board->write_packet)) { + LL_PRINT("ERROR: send_queued_writes: invalid batch, dropping all data\n"); //We need to drop the data to recover board->write_packet_ptr = board->write_packet; + board->write_queue_error = false; + board->write_batch_dropped = true; return 0; } @@ -1167,34 +1232,47 @@ static int hm2_eth_send_queued_writes(hm2_lowlevel_io_t *this) { board->has_written_cnt = 1; t0 = rtapi_get_time(); - send = eth_socket_send(board, (void*) &board->write_packet, board->write_packet_ptr - board->write_packet); - if(send < 0) { - LL_PRINT("ERROR: sending packet: %s\n", strerror(errno)); + size_t packet_size = board->write_packet_ptr - board->write_packet; + send = eth_socket_send(board, (void*) &board->write_packet, packet_size); + if(send != (int)packet_size) { + LL_PRINT("ERROR: sending queued-write packet%s%s\n", + send < 0 ? ": " : "", + send < 0 ? strerror(errno) : ""); } t1 = rtapi_get_time(); LL_PRINT_IF(debug, "enqueue_write(%d) : PACKET SEND [SIZE: %d | TIME: %lld]\n", board->write_cnt, send, t1 - t0); board->write_packet_ptr = board->write_packet; - return send < 0 ? 0 : 1; + return send == (int)packet_size; } static int hm2_eth_enqueue_write(hm2_lowlevel_io_t *this, rtapi_u32 addr, const void *buffer, int size) { hm2_eth_t *board = this->private; if (comm_active == 0) return 1; if (size == 0) return 1; - lbp16_cmd_addr *packet = (lbp16_cmd_addr *) board->write_packet_ptr; - - //Check size we are going to write + if (board->write_queue_error) return 0; + if (!hm2_eth_valid_transfer_size(size)) { + THIS_ERR("hm2_eth_enqueue_write: invalid transfer size %d\n", size); + goto fail; + } size_t write_packet_size = board->write_packet_ptr - board->write_packet; - if (write_packet_size + sizeof(*packet) + size > sizeof(board->write_packet)) { - LL_PRINT("ERROR: enqueue_write: buffer full\n"); - return 0; + size_t write_packet_trailer_size = + sizeof(lbp16_cmd_addr) + sizeof(board->write_cnt); + if (write_packet_size + sizeof(lbp16_cmd_addr) + size + + write_packet_trailer_size > sizeof(board->write_packet)) { + THIS_ERR("hm2_eth_enqueue_write: write packet buffer size exceeded\n"); + goto fail; } + lbp16_cmd_addr *packet = (lbp16_cmd_addr *) board->write_packet_ptr; LBP16_INIT_PACKET4_PTR(packet, CMD_WRITE_HOSTMOT2_ADDR32_INCR(size/4), addr); board->write_packet_ptr += sizeof(*packet); memcpy(board->write_packet_ptr, buffer, size); board->write_packet_ptr += size; return 1; + +fail: + board->write_queue_error = true; + return 0; } static int hm2_eth_set_force_enqueue(hm2_lowlevel_io_t *this, int do_enqueue) { @@ -1218,19 +1296,19 @@ static int hm2_eth_probe(hm2_eth_t *board) { lbp16_cmd_addr read_packet; int ret, send, recv; - char board_name[16] = {}; - char llio_name[16] = {}; + char board_name[16] = {0}; + char llio_name[16] = {0}; LBP16_INIT_PACKET4(read_packet, CMD_READ_BOARD_INFO_ADDR16_INCR(16/2), 0); send = eth_socket_send(board, (void*) &read_packet, sizeof(read_packet)); - if(send < 0) { + if(send != (int)sizeof(read_packet)) { LL_PRINT("ERROR: sending packet: %s\n", strerror(errno)); - return -errno; + return send < 0 ? -errno : -EIO; } recv = eth_socket_recv(board, (void*) &board_name, 16, RECV_TIMEOUT_NON_RT_NS); - if(recv < 0) { + if(recv != (int)sizeof(board_name)) { LL_PRINT("ERROR: receiving packet: %s\n", strerror(errno)); - return -errno; + return recv < 0 ? -errno : -EIO; } board = &boards[boards_count]; @@ -1579,12 +1657,14 @@ static int hm2_eth_probe(hm2_eth_t *board) { // (such as 0 or -1) could be passed here and the layer which can // legitimately read idroms would read the values and store them, but // that wasn't trivial to do. - rtapi_u32 read_data; - hm2_eth_read(&board->llio, HM2_ADDR_IDROM_OFFSET, &read_data, 4); + rtapi_u32 read_data = 0; + if (!hm2_eth_read(&board->llio, HM2_ADDR_IDROM_OFFSET, &read_data, sizeof(read_data))) + return -EIO; unsigned int idrom_address = read_data & 0xffff; hm2_idrom_t idrom; memset(&idrom, 0, sizeof(idrom)); - hm2_eth_read(&board->llio, idrom_address, &idrom, sizeof(idrom)); + if (!hm2_eth_read(&board->llio, idrom_address, &idrom, sizeof(idrom))) + return -EIO; board->llio.num_ioport_connectors = idrom.io_ports; board->llio.pins_per_connector = idrom.port_width; diff --git a/src/hal/drivers/mesa-hostmot2/hm2_eth.h b/src/hal/drivers/mesa-hostmot2/hm2_eth.h index 0741e8ff0dc..804bb16a358 100644 --- a/src/hal/drivers/mesa-hostmot2/hm2_eth.h +++ b/src/hal/drivers/mesa-hostmot2/hm2_eth.h @@ -28,6 +28,7 @@ #define HM2_LLIO_NAME "hm2_eth" #define MAX_ETH_READS 64 +#define HM2_ETH_PACKET_SIZE 1400 typedef struct { void *buffer; @@ -53,14 +54,17 @@ struct hm2_eth_t { int (*eth_socket_send)(hm2_eth_t *board, const void *buffer, int len); int (*eth_socket_recv)(hm2_eth_t *board, void *buffer, int len, int recv_timeout_ns); - rtapi_u8 read_packet[1400]; + rtapi_u8 read_packet[HM2_ETH_PACKET_SIZE]; rtapi_u8 *read_packet_ptr; hm2_read_queue_entry_t queue_reads[MAX_ETH_READS]; int queue_reads_count; int queue_buff_size; + bool read_queue_error; - rtapi_u8 write_packet[1400]; + rtapi_u8 write_packet[HM2_ETH_PACKET_SIZE]; rtapi_u8 *write_packet_ptr; + bool write_queue_error; + bool write_batch_dropped; // Retained across later flushes until receive reports it. uint32_t read_cnt, write_cnt; struct { // These two fields must be kept together. They are read by a single