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;
}