static ssize_t _thsafe_zmq_client_write_generic (smio_t *self, loff_t offs, const uint8_t *data, uint32_t size) { assert (self); ssize_t ret_size = -1; zmsg_t *send_msg = zmsg_new (); ASSERT_ALLOC(send_msg, err_msg_alloc); uint32_t opcode; DBE_DEBUG (DBG_MSG | DBG_LVL_TRACE, "[smio_thsafe_client:zmq] Calling _thsafe_write_generic\n"); switch (size) { case THSAFE_WRITE_16_DSIZE: opcode = THSAFE_WRITE_16; break; case THSAFE_WRITE_32_DSIZE: opcode = THSAFE_WRITE_32; break; case THSAFE_WRITE_64_DSIZE: opcode = THSAFE_WRITE_64; break; default: opcode = THSAFE_WRITE_32; } /* Message is: * frame 0: WRITE<size> opcode * frame 1: offset * frame 2: data to be written * */ int zerr = zmsg_addmem (send_msg, &opcode, sizeof (opcode)); ASSERT_TEST(zerr == 0, "Could not add WRITE opcode in message", err_add_opcode); zerr = zmsg_addmem (send_msg, &offs, sizeof (offs)); ASSERT_TEST(zerr == 0, "Could not add offset in message", err_add_offset); zerr = zmsg_addmem (send_msg, data, size); ASSERT_TEST(zerr == 0, "Could not add READ opcode in message", err_add_data); DBE_DEBUG (DBG_MSG | DBG_LVL_TRACE, "[smio_thsafe_client:zmq] Sending message:\n"); #ifdef LOCAL_MSG_DBG zmsg_print (send_msg); #endif zerr = zmsg_send (&send_msg, self->pipe); ASSERT_TEST(zerr == 0, "Could not send message", err_send_msg); /* Message is: * frame 0: reply code * frame 1: return code */ ret_size = _thsafe_zmq_client_recv_write (self); err_send_msg: err_add_data: err_add_offset: err_add_opcode: zmsg_destroy (&send_msg); err_msg_alloc: return ret_size; }
int _thsafe_zmq_client_open_release (smio_t *self, llio_endpoint_t *endpoint, uint32_t opcode) { assert (self); int ret = -1; zmsg_t *send_msg = zmsg_new (); ASSERT_ALLOC(send_msg, err_msg_alloc); DBE_DEBUG (DBG_MSG | DBG_LVL_TRACE, "[smio_thsafe_client:zmq] Calling thsafe_release\n"); /* Message is: * frame 0: RELEASE opcode * frame 1: endpopint struct (FIXME?) */ int zerr = zmsg_addmem (send_msg, &opcode, sizeof (opcode)); ASSERT_TEST(zerr == 0, "Could not add OPEN opcode in message", err_add_opcode); zerr = zmsg_addmem (send_msg, endpoint, sizeof (*endpoint)); ASSERT_TEST(zerr == 0, "Could not add endpoint in message", err_add_endpoint); DBE_DEBUG (DBG_MSG | DBG_LVL_TRACE, "[smio_thsafe_client:zmq] Sending message:\n"); #ifdef LOCAL_MSG_DBG zmsg_print (send_msg); #endif zerr = zmsg_send (&send_msg, self->pipe); ASSERT_TEST(zerr == 0, "Could not send message", err_send_msg); /* Message is: * frame 0: reply code * frame 1: return code */ /* Returns NULL if confirmation was not OK or in case of error. * Returns the original message if the confirmation was OK */ zmsg_t *recv_msg = _thsafe_zmq_client_recv_confirmation (self); ASSERT_TEST(recv_msg != NULL, "Could not receive confirmation code", err_null_raw_data); /* If we are here the message got a OK reply code. * Just return the return code */ zframe_t *reply_frame = zmsg_pop (recv_msg); zframe_destroy (&reply_frame); /* Don't do anything with the reply code */ zframe_t *ret_code_frame = zmsg_pop (recv_msg); if (ret_code_frame == NULL) { DBE_DEBUG (DBG_MSG | DBG_LVL_TRACE, "[smio_thsafe_client:zmq] Interrupted or malformed message\n"); /* Interrupted or malformed message */ goto err_recv_data; } /* Check if the frame has the number of bytes requested. * For now, we consider a success only when the number of * bytes requested is the same as the actually read*/ if (zframe_size (ret_code_frame) != THSAFE_REPLY_SIZE) { DBE_DEBUG (DBG_MSG | DBG_LVL_TRACE, "[smio_thsafe_client:zmq] Frame size is wrong\n"); goto err_recv_data_size; } ret = *(THSAFE_REPLY_TYPE *) zframe_data (ret_code_frame); DBE_DEBUG (DBG_MSG | DBG_LVL_TRACE, "[smio_thsafe_client:zmq] Received return code: %08X\n", ret); err_recv_data_size: zframe_destroy (&ret_code_frame); err_recv_data: err_null_raw_data: zmsg_destroy (&recv_msg); err_send_msg: err_add_endpoint: err_add_opcode: zmsg_destroy (&send_msg); err_msg_alloc: return ret; }
/* Register an specific sm_io modules to this device */ devio_err_e devio_register_sm (devio_t *self, uint32_t smio_id, void *priv) { assert (self); /* Search the sm_io_mod_dsapatch table for the smio_id and, * if found, call the correspondent bootstrap code to initilize * the sm_io module */ th_boot_args_t *th_args = NULL; /* For now, just do a simple linear search. We can afford this, as * we don't expect to insert new sm_io modules often */ unsigned int i; DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io_core:register_sm] smio_mod_dispatch table size = %ld\n", ARRAY_SIZE(smio_mod_dispatch)); for (i = 0; i < ARRAY_SIZE(smio_mod_dispatch); ++i) { if (smio_mod_dispatch[i].id == smio_id) { /* Found! Call bootstrap code and insert in * hash table */ /* FIXME: Why do I need this? smio always gets initilized * after smio_mod_dispatch[i].bootstrap_ops->smio_boot (self); */ /* smio_t *smio = NULL; */ /* It is expected tha after the boot () call the operations * this sm_io inscate can handle are already registered! */ DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io_core:register_sm] Allocating thread args\n"); /* Alloacate thread arguments struct and pass it to the * thread. It is the responsability of the calling thread * to clear this structure after using it! */ th_boot_args_t *th_args = zmalloc (sizeof *th_args); ASSERT_ALLOC (th_args, err_th_args_alloc); th_args->parent = self; /* FIXME: weak identifier */ th_args->smio_id = i; th_args->broker = self->endpoint_broker; th_args->service = self->name; th_args->verbose = self->verbose; th_args->priv = priv; DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io_core:register_sm] Calling boot func\n"); uint32_t pipe_idx = self->nnodes++; self->pipes [pipe_idx] = zthread_fork (self->ctx, smio_startup, th_args); /* self->pipes [pipe_idx] = zthread_fork (self->ctx, smio_mod_dispatch[i].bootstrap_ops->thread_boot, th_args); */ /*smio = smio_mod_dispatch[i].bootstrap_ops->boot (self);*/ /*ASSERT_ALLOC (smio, err_smio_alloc); */ /* Stringify ID */ DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io_core:register_sm] Stringify hash ID\n"); char *key = halutils_stringify_key (smio_mod_dispatch[i].id); ASSERT_ALLOC (key, err_key_alloc); DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io_core:register_sm] Inserting hash with key: %s\n", key); zhash_insert (self->sm_io_h, key, self->pipes [pipe_idx]); free (key); /* stop on first match */ break; } } //free (th_args); return DEVIO_SUCCESS; err_key_alloc: free (th_args); err_th_args_alloc: return DEVIO_ERR_ALLOC; }
int main (int argc, char *argv[]) { int verbose = 0; int daemonize = 0; char *dev_type = NULL; char *dev_entry = NULL; char *broker_endp = NULL; char **str_p = &dev_type; /* default */ int i; if (argc < 4) { print_help (argv[0]); exit (1); } /* FIXME: This is rather buggy! */ /* Simple handling of command-line options. This should be done * with getopt, for instance*/ for (i = 1; i < argc; i++) { if (streq (argv[i], "-v")) { verbose = 1; DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io] Verbose mode set\n"); } else if (streq (argv[i], "-d")) { daemonize = 1; DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io] Demonize mode set\n"); } else if (streq (argv[i], "-t")) { str_p = &dev_type; DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io] Will set dev_type parameter\n"); } else if (streq (argv[i], "-e")) { str_p = &dev_entry; DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io] Will set dev_entry parameter\n"); } else if (streq (argv[i], "-b")) { str_p = &broker_endp; DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io] Will set broker_endp parameter\n"); } else if (streq (argv[i], "-h")) { print_help (argv[0]); exit (1); } /* Fallout for options with parameters */ else { *str_p = strdup (argv[i]); DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io] Parameter set to \"%s\"\n", *str_p); } } /* Daemonize dev_io */ if (daemonize != 0) { int rc = daemon(0, 0); if (rc != 0) { perror ("[dev_io] daemon"); goto err_exit; } } llio_type_e llio_type; /* Parse command-line options */ if (streq (dev_type, "pcie")) { llio_type = PCIE_DEV; } else if (streq (dev_type, "eth")) { llio_type = ETH_DEV; } else { DBE_DEBUG (DBG_DEV_IO | DBG_LVL_FATAL, "[dev_io] Dev_type parameter is invalid\n"); goto err_exit; } /* We don't need it anymore */ str_p = &dev_type; free (*str_p); dev_type = NULL; /* Initilialize dev_io */ DBE_DEBUG (DBG_DEV_IO | DBG_LVL_TRACE, "[dev_io] Creating devio instance ...\n"); devio_t *devio = devio_new ("BPM0:DEVIO", dev_entry, llio_type, broker_endp, verbose); /* devio_t *devio = devio_new ("BPM0:DEVIO", *str_p, llio_type, "tcp://localhost:5555", verbose); */ if (devio == NULL) { DBE_DEBUG (DBG_DEV_IO | DBG_LVL_FATAL, "[dev_io] devio_new error!\n"); goto err_exit; } /* We don't need it anymore */ str_p = &dev_entry; free (*str_p); dev_entry = NULL; str_p = &broker_endp; free (*str_p); broker_endp = NULL; uint32_t acq_id = 0x4519a0ad; uint32_t fmc130m_4ch_id = 0x7085ef15; devio_err_e err; err = devio_register_sm (devio, acq_id, NULL); if (err != DEVIO_SUCCESS) { DBE_DEBUG (DBG_DEV_IO | DBG_LVL_FATAL, "[dev_io] devio_register_sm error!\n"); goto err_devio; } err = devio_register_sm (devio, fmc130m_4ch_id, NULL); if (err != DEVIO_SUCCESS) { DBE_DEBUG (DBG_DEV_IO | DBG_LVL_FATAL, "[dev_io] devio_register_sm error!\n"); goto err_devio; } err = devio_init_poller_sm (devio); if (err != DEVIO_SUCCESS) { DBE_DEBUG (DBG_DEV_IO | DBG_LVL_FATAL, "[dev_io] devio_init_poller_sm error!\n"); goto err_devio; } while (!zctx_interrupted) { /* Step 1: Loop though all the SDB records and intialize (boot) the * smio modules*/ /* Step 2: Optionally, register the necessary smio modules specifying * its ID and calling devio_register_sm */ /* Step 3: Poll all PIPE's sockets to determine if we have something to * handle, like messages from smios */ /* Step 3.5: If we do, call devio_handle_smio () and treat its * request as appropriate */ devio_poll_all_sm (devio); } err_devio: devio_destroy (&devio); err_exit: free (broker_endp); free (dev_entry); free (dev_type); return 0; }
static ssize_t _pcie_rw_block (llio_t *self, uint64_t offs, size_t size, uint32_t *data, int rw) { assert (self); int err = 0; ASSERT_TEST(llio_get_endpoint_open (self), "Could not perform RW operation. Device is not opened", err_endp_open, -1); llio_dev_pcie_t *dev_pcie = llio_get_dev_handler (self); ASSERT_TEST(dev_pcie != NULL, "Could not get PCIe handler", err_dev_pcie_handler, -1); /* Determine which bar to operate on */ int bar_no = PCIE_ADDR_BAR (offs); uint64_t full_offs = PCIE_ADDR_GEN (offs); int pg_start; uint64_t pg_offs; switch (bar_no) { /* PCIe config registers */ case BAR0NO: /* Not available */ break; /* FPGA SDRAM */ case BAR2NO: DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "----------------------------------------------------------\n" "[ll_io_pcie:_pcie_rw_block] Going to read/write in BAR2\n"); pg_start = PCIE_ADDR_SDRAM_PG (full_offs); pg_offs = PCIE_ADDR_SDRAM_PG_OFFS (full_offs); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_block] bar_no = %d, pg_start = %d,\n\tfull_offs = 0x%lx, pg_offs = 0x%lx\n", bar_no, pg_start, full_offs, pg_offs); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_block] full_addr = 0x%p\n" "-------------------------------------------------------------------------------------\n", dev_pcie->bar2 + pg_offs); err = _pcie_rw_bar2_block_td (self, pg_start, pg_offs, data, size, rw); break; /* FPGA Wishbone */ case BAR4NO: DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "----------------------------------------------------------\n" "[ll_io_pcie:_pcie_rw_block] Going to read/write in BAR4\n"); pg_start = PCIE_ADDR_WB_PG (full_offs); pg_offs = PCIE_ADDR_WB_PG_OFFS (full_offs); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_block] bar_no = %d, pg_start = %d,\n\tfull_offs = 0x%lx, pg_offs = 0x%lx\n", bar_no, pg_start, full_offs, pg_offs); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_block] full_addr = %p\n" "-------------------------------------------------------------------------------------\n", dev_pcie->bar4 + pg_offs); err = _pcie_rw_bar4_block_td (self, pg_start, pg_offs, data, size, rw); break; /* Invalid BAR */ default: DBE_DEBUG (DBG_LL_IO | DBG_LVL_ERR, "----------------------------------------------------------\n" "[ll_io_pcie:_pcie_rw_32] Invalid BAR access\n"); break; } err_dev_pcie_handler: err_endp_open: return err; }
/************ Helper functions **********/ static ssize_t _pcie_rw_32 (llio_t *self, uint64_t offs, uint32_t *data, int rw) { assert (self); int err = sizeof (*data); ASSERT_TEST(llio_get_endpoint_open (self), "Could not perform RW operation. Device is not opened", err_endp_open, -1); llio_dev_pcie_t *dev_pcie = llio_get_dev_handler (self); ASSERT_TEST(dev_pcie != NULL, "Could not get PCIe handler", err_dev_pcie_handler, -1); /* Determine which bar to operate on */ int bar_no = PCIE_ADDR_BAR (offs); uint64_t full_offs = PCIE_ADDR_GEN (offs); int pg_num; uint64_t pg_offs; /* FIXME: This switch is in the critical path! Remove it */ switch (bar_no) { /* PCIe config registers */ case BAR0NO: DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "----------------------------------------------------------\n" "[ll_io_pcie:_pcie_rw_32] Going to read/write in BAR0\n"); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_32] bar_no = %d, full_offs = %lX\n" "-------------------------------------------------------------------------------------\n", bar_no, full_offs); BAR0_RW(dev_pcie->bar0, full_offs, data, rw); break; /* FPGA SDRAM */ case BAR2NO: DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "----------------------------------------------------------\n" "[ll_io_pcie:_pcie_rw_32] Going to read/write in BAR2\n"); pg_num = PCIE_ADDR_SDRAM_PG (full_offs); pg_offs = PCIE_ADDR_SDRAM_PG_OFFS (full_offs); SET_SDRAM_PG (dev_pcie->bar0, pg_num); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_32] bar_no = %d, pg_num = %d,\n\tfull_offs = 0x%lx, pg_offs = 0x%lx\n", bar_no, pg_num, full_offs, pg_offs); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_32] full_addr = 0x%p\n" "-------------------------------------------------------------------------------------\n", ((llio_dev_pcie_t *) llio_get_dev_handler (self))->bar2 + pg_offs); BAR2_RW(dev_pcie->bar2, pg_offs, data, rw); break; /* FPGA Wishbone */ case BAR4NO: DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "----------------------------------------------------------\n" "[ll_io_pcie:_pcie_rw_32] Going to read/write in BAR4\n"); pg_num = PCIE_ADDR_WB_PG (full_offs); pg_offs = PCIE_ADDR_WB_PG_OFFS (full_offs); SET_WB_PG (dev_pcie->bar0, pg_num); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_32] bar_no = %d, pg_num = %d,\n\tfull_offs = 0x%lx, pg_offs = 0x%lx\n", bar_no, pg_num, full_offs, pg_offs); DBE_DEBUG (DBG_LL_IO | DBG_LVL_TRACE, "[ll_io_pcie:_pcie_rw_32] full_addr = %p\n" "-------------------------------------------------------------------------------------\n", ((llio_dev_pcie_t *) llio_get_dev_handler (self))->bar4 + pg_offs); BAR4_RW(dev_pcie->bar4, pg_offs, data, rw); break; /* Invalid BAR */ default: DBE_DEBUG (DBG_LL_IO | DBG_LVL_ERR, "----------------------------------------------------------\n" "[ll_io_pcie:_pcie_rw_32] Invalid BAR access\n"); return -1; } err_dev_pcie_handler: err_endp_open: return err; }