You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
 
 
 
 
 
 

833 lines
21 KiB

/*
Copyright (c) 2013,
Daan Vreeken <Daan @ Vitsch . nl> - Vitsch Electronics
Copyright Jeroen Vreeken (pe1rxq@amsat.org), 2013
Copyright Stichting C.A. Muller Radioastronomiestation, 2013
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/>.
*/
/*
esc_esi.c
Code for accessing the ESI data stored in the device's eeprom.
The ESI data contains a device description which can be used during
initialization.
*/
#include <string.h>
#include <ec/ec.h>
#include <ec/esc.h>
#include <ec/esc_esi.h>
#include <ec/esc_dc.h>
#include <ec/esc_registers.h>
#include <ec/esc_mailbox.h>
#include <ec/esc_watchdog.h>
#include <log/log.h>
#include <controller/controller_time.h>
#define MAX_RETRIES 1000
ssize_t esc_esi_eeprom_read(struct ec *ec, struct ec_dgram_addr *ec_addr,
void *buffer, size_t offset, size_t size)
{
int r;
uint8_t val8;
uint16_t val16, status;
uint32_t val32;
int wordsize;
size_t readsize = 0;
int retry = 0;
/* See ET1100 pdf, chapter 11.2.2: Assignment to ECAT/PDI */
/* Request use of EEPROM by master (us) */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_REQUEST_ECAT;
r = ec_datagram_write(ec, ec_addr, &val8, 1);
if (r != 1)
return -1;
/* Make sure ecat side has eeprom control */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_PDI_STATE;
do {
r = ec_datagram_read(ec, ec_addr, &val8, 1);
} while (val8 == ESC_ESI_EEPROM_PDI_BUSY && r == 1 &&
++retry < MAX_RETRIES);
if (r != 1) {
readsize = -1;
goto err_out;
}
if (retry == MAX_RETRIES) {
log_send(LOG_T_DEBUG, "esc_esi_eeprom_read: forcing the PDI "
"off the EEPROM bus");
/* Force the PDI off of the EEPROM bus */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_FORCE_ECAT;
r = ec_datagram_write(ec, ec_addr, &val8, 1);
if (r != 1) {
readsize = -1;
goto err_out;
}
/* Finish claiming the bus */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_REQUEST_ECAT;
r = ec_datagram_write(ec, ec_addr, &val8, 1);
if (r != 1) {
readsize = -1;
goto err_out;
}
}
/* get status */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONTROL_STATUS;
r = ec_datagram_read(ec, ec_addr, &val16, 2);
if (r != 2) {
readsize = -1;
goto err_out;
}
status = le16toh(val16);
if (status & ESC_ESI_EEPROM_CONTROL_STATUS_READ_8B) {
wordsize = 8;
} else {
wordsize = 4;
}
while (size) {
uint8_t data[8];
size_t copysize;
/* set address */
val32 = htole32(offset / 2);
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_ADDRESS;
r = ec_datagram_write(ec, ec_addr, &val32, 4);
/* Wait for other stuff to finish */
do {
r = ec_datagram_read(ec, ec_addr, &val16, 2);
} while (le16toh(val16) & ESC_ESI_EEPROM_CONTROL_STATUS_BUSY && r == 2);
/* Issue read command */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONTROL_STATUS;
val16 = htole16(ESC_ESI_EEPROM_CONTROL_STATUS_CMD_READ);
r = ec_datagram_write(ec, ec_addr, &val16, 2);
/* Wait for read to finish */
do {
r = ec_datagram_read(ec, ec_addr, &val16, 2);
} while (le16toh(val16) & ESC_ESI_EEPROM_CONTROL_STATUS_BUSY && r == 2);
/* make sure nothing is fucked up.... */
status = le16toh(val16);
if (status & ESC_ESI_EEPROM_CONTROL_STATUS_CHKSUM_ERR ||
status & ESC_ESI_EEPROM_CONTROL_STATUS_NOT_LOADED ||
status & ESC_ESI_EEPROM_CONTROL_STATUS_ACK_ERR ||
status & ESC_ESI_EEPROM_CONTROL_STATUS_WRITE_ERR) {
readsize = -1;
goto err_out;
}
/* read data and copy to client buffer */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_DATA;
if (size < wordsize)
wordsize = size;
r = ec_datagram_read(ec, ec_addr, data, wordsize);
if (r != wordsize)
break;
copysize = size;
if (copysize > wordsize)
copysize = wordsize;
memcpy(buffer, data, copysize);
size -= copysize;
offset += copysize;
buffer += copysize;
readsize += copysize;
}
err_out:
/* Give back the EEPROM to the PDI */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_PDI;
/* if this fails all hope is lost for the PDI :) */
(void)ec_datagram_write(ec, ec_addr, &val8, 1);
return readsize;
}
ssize_t esc_esi_eeprom_write(struct ec *ec, struct ec_dgram_addr *ec_addr,
void *buffer, size_t offset, size_t size)
{
int r;
uint8_t val8;
uint16_t val16, status;
uint32_t val32;
size_t i;
int retry = 0;
ssize_t writesize = 0;
/* See ET1100 pdf, chapter 11.2.2: Assignment to ECAT/PDI */
/* Request use of EEPROM by master (us) */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_REQUEST_ECAT;
r = ec_datagram_write(ec, ec_addr, &val8, 1);
if (r != 1)
return -1;
/* Make sure ecat side has eeprom control */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_PDI_STATE;
do {
r = ec_datagram_read(ec, ec_addr, &val8, 1);
} while (val8 == ESC_ESI_EEPROM_PDI_BUSY && r == 1 && ++retry < MAX_RETRIES);
if (r != 1) {
writesize = -1;
goto err_out;
}
if (retry == MAX_RETRIES) {
log_send(LOG_T_DEBUG, "esc_esi_eeprom_write: forcing the PDI off the EEPROM bus");
/* Force the PDI off of the EEPROM bus */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_FORCE_ECAT;
r = ec_datagram_write(ec, ec_addr, &val8, 1);
if (r != 1) {
writesize = -1;
goto err_out;
}
/* Finish claiming the bus */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_REQUEST_ECAT;
r = ec_datagram_write(ec, ec_addr, &val8, 1);
if (r != 1) {
writesize = -1;
goto err_out;
}
}
/* get status */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONTROL_STATUS;
r = ec_datagram_read(ec, ec_addr, &val16, 2);
if (r != 2) {
writesize = -1;
goto err_out;
}
status = le16toh(val16);
for (i = 0; i < size; i += 2) {
/* set address, word addressing... */
val32 = htole32((offset + i)/ 2);
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_ADDRESS;
r = ec_datagram_write(ec, ec_addr, &val32, 4);
/* Wait for other stuff to finish */
do {
r = ec_datagram_read(ec, ec_addr, &val16, 2);
} while (le16toh(val16) & ESC_ESI_EEPROM_CONTROL_STATUS_BUSY && r == 2);
/* write data */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_DATA;
void *data = ((char*)buffer) + offset + i;
r = ec_datagram_write(ec, ec_addr, data, 2);
if (r != 2)
break;
/* Issue write command */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONTROL_STATUS;
val16 = htole16(ESC_ESI_EEPROM_CONTROL_STATUS_ECAT_WRITE_ENABLE | ESC_ESI_EEPROM_CONTROL_STATUS_CMD_WRITE);
r = ec_datagram_write(ec, ec_addr, &val16, 2);
/* Wait for write to finish */
do {
r = ec_datagram_read(ec, ec_addr, &val16, 2);
} while (le16toh(val16) & ESC_ESI_EEPROM_CONTROL_STATUS_BUSY && r == 2);
/* make sure nothing is fucked up.... */
status = le16toh(val16);
if (status & ESC_ESI_EEPROM_CONTROL_STATUS_CHKSUM_ERR ||
status & ESC_ESI_EEPROM_CONTROL_STATUS_NOT_LOADED ||
status & ESC_ESI_EEPROM_CONTROL_STATUS_ACK_ERR ||
status & ESC_ESI_EEPROM_CONTROL_STATUS_WRITE_ERR) {
writesize = -1;
goto err_out;
}
writesize += 2;
}
err_out:
/* Give back the EEPROM to the PDI */
ec_addr->addr.position.off = ESC_ADDR_MAP_ESI_EEPROM_CONFIG;
val8 = ESC_ESI_EEPROM_CONFIG_PDI;
/* if this fails all hope is lost for the PDI :) */
(void)ec_datagram_write(ec, ec_addr, &val8, 1);
return writesize;
}
uint16_t esc_esi_read16(struct ec *ec, struct ec_dgram_addr *ec_addr, size_t offset)
{
uint16_t val16 = 0;
esc_esi_eeprom_read(ec, ec_addr, &val16, offset, sizeof(uint16_t));
return le16toh(val16);
}
uint32_t esc_esi_read32(struct ec *ec, struct ec_dgram_addr *ec_addr, size_t offset)
{
uint32_t val32 = 0;
esc_esi_eeprom_read(ec, ec_addr, &val32, offset, sizeof(uint32_t));
return le32toh(val32);
}
int esc_esi_write16(struct ec *ec, struct ec_dgram_addr *ec_addr, size_t offset, uint16_t val)
{
val = htole16(val);
ssize_t r = esc_esi_eeprom_write(ec, ec_addr, &val, offset, sizeof(uint16_t));
if (r == 2)
return 0;
return -1;
}
uint8_t esc_esi_crc8(void *data, size_t size)
{
uint8_t r = 0xff;
uint8_t pol = 0x07;
uint8_t *datab = data;
size_t i;
int bit;
for (i=0; i < size; i++) {
r ^= datab[i];
for (bit = 0; bit < 8; bit++) {
if (r & 0x80)
r = (r << 1) ^ pol;
else
r = (r << 1);
}
}
return r;
}
size_t esc_esi_standard_rx_mailbox_offset_get(struct ec *ec, struct ec_dgram_addr *ec_addr)
{
return esc_esi_read16(ec, ec_addr, ESC_ESI_STANDARD_RX_MAILBOX_OFFSET);
}
size_t esc_esi_standard_tx_mailbox_offset_get(struct ec *ec, struct ec_dgram_addr *ec_addr)
{
return esc_esi_read16(ec, ec_addr, ESC_ESI_STANDARD_TX_MAILBOX_OFFSET);
}
size_t esc_esi_standard_rx_mailbox_size_get(struct ec *ec, struct ec_dgram_addr *ec_addr)
{
return esc_esi_read16(ec, ec_addr, ESC_ESI_STANDARD_RX_MAILBOX_SIZE);
}
size_t esc_esi_standard_tx_mailbox_size_get(struct ec *ec, struct ec_dgram_addr *ec_addr)
{
return esc_esi_read16(ec, ec_addr, ESC_ESI_STANDARD_TX_MAILBOX_SIZE);
}
uint32_t esc_esi_vendorid_get(struct esc_device *esc)
{
return esc_esi_read32(esc->ec, &esc->addr, ESC_ESI_ADDR_VENDORID);
}
uint32_t esc_esi_productcode_get(struct esc_device *esc)
{
return esc_esi_read32(esc->ec, &esc->addr, ESC_ESI_ADDR_PRODUCTCODE);
}
uint32_t esc_esi_revisionno_get(struct esc_device *esc)
{
return esc_esi_read32(esc->ec, &esc->addr, ESC_ESI_ADDR_REVISIONNO);
}
uint32_t esc_esi_serialno_get(struct esc_device *esc)
{
return esc_esi_read32(esc->ec, &esc->addr, ESC_ESI_ADDR_SERIALNO);
}
struct esc_esi_category *esc_esi_categories_create(struct ec *ec, struct ec_dgram_addr *ec_addr)
{
struct esc_esi_category *categories = NULL, *cat, **catp;
size_t word_offset;
int max_cat = 100;
ssize_t r;
word_offset = ESC_ESI_FIRST_CATEGORY_TYPE/2;
do {
uint16_t type;
uint16_t size;
type = esc_esi_read16(ec, ec_addr, word_offset*2);
size = esc_esi_read16(ec, ec_addr, (word_offset+1)*2);
if (type == 0xffff) {
break;
}
cat = calloc(1, sizeof(struct esc_esi_category));
if (!cat)
break;
cat->length = size*2;
cat->type = type;
cat->data = calloc(1, cat->length);
if (!cat->data) {
free(cat);
break;
}
r = esc_esi_eeprom_read(ec, ec_addr, cat->data, (word_offset+2)*2,
cat->length);
if (r != cat->length) {
free(cat->data);
free(cat);
break;
}
word_offset += size + 2;
for (catp = &categories; *catp; catp = &(*catp)->next);
(*catp) = cat;
} while (max_cat--);
return categories;
}
void esc_esi_categories_destroy(struct esc_esi_category *cat)
{
struct esc_esi_category *old;
while (cat) {
old = cat;
cat = old->next;
free(old->data);
free(old);
}
}
ssize_t esc_esi_category_strings_get(struct esc_esi_category *categories,
int nr, char *str, size_t len)
{
struct esc_esi_category *cat;
ssize_t r = -1;
uint8_t nr_strings;
int cur;
size_t offset;
if (len > 1)
str[0] = 0;
for (cat = categories; cat && cat->type != ESC_ESI_CATEGORY_STRINGS;
cat = cat->next);
if (!cat || cat->length < 2)
return r;
nr_strings = cat->data[0];
if (nr > nr_strings)
return r;
offset = 1;
cur = 1;
while (offset < cat->length && cur < nr_strings && cur != nr) {
cur++;
offset += cat->data[offset] + 1;
}
if (cur == nr) {
size_t copysize;
copysize = cat->data[offset];
if (copysize + offset > cat->length)
copysize = cat->length - offset;
if (copysize > len-1)
copysize = len-1;
memcpy(str, &cat->data[offset+1], copysize);
str[copysize] = 0;
r = copysize;
}
return r;
}
struct esc_esi_syncmanager {
uint16_t offset;
uint16_t size;
uint8_t ctrl;
uint8_t stat;
uint8_t enabled;
uint8_t unknown1; /* pdictrl ?*/
} __packed;
struct esc_esi_pdo {
uint16_t index;
uint8_t entries;
uint8_t sm;
uint8_t unknown1;
uint8_t string;
uint8_t unknown2;
uint8_t unknown3;
} __packed;
struct esc_esi_pdo_entry {
uint16_t index;
uint8_t unknown1; /* probably sub-index */
uint8_t string;
uint8_t unknown2;
uint8_t bitsize;
uint8_t unknown3;
uint8_t unknown4;
} __packed;
struct esc_esi_fmmu {
uint8_t unknown1;
} __packed;
struct esc_esi_dcclock {
uint32_t cycle_time_sync0;
int32_t shift_time_sync0;
int32_t shift_time_sync1;
int16_t cycle_time_sync1_factor;
uint16_t assign_activate;
int16_t cycle_time_sync0_factor;
uint8_t dc_sync_name;
uint8_t unknown1;
uint8_t unknown2;
uint8_t unknown3;
uint8_t unknown4;
uint8_t unknown5;
} __packed;
static int esc_esi_device_fill_dc(struct esc_device *dev)
{
struct esc_esi_category *cat;
struct esc_esi_dcclock *dc;
int i, nr;
double period;
uint32_t period_cycle;
struct controller_time *t = NULL;
if (dev->block)
t = dev->block->time;
period = controller_time_period_get(t);
period_cycle = (uint32_t)(period * 1000 * 1000 * 1000);
for (cat = dev->categories;
cat && cat->type != ESC_ESI_CATEGORY_DCCLOCK;
cat = cat->next);
if (!cat)
return -1;
if (cat->length % sizeof(struct esc_esi_dcclock)) {
log_send(LOG_T_ERROR,
"SII DC clock data is not a multiple of %zd",
sizeof(struct esc_esi_dcclock));
return -1;
}
nr = cat->length / sizeof(struct esc_esi_dcclock);
dc = (struct esc_esi_dcclock *)cat->data;
for (i = 0; i < nr; i++) {
char string[200];
uint8_t activate, cyclic;
uint32_t cycle0;
int32_t shift0, shift1;
int16_t factor0, factor1;
bool sync_out, sync0, sync1, latch0, latch1;
cycle0 = le32toh(dc->cycle_time_sync0);
shift0 = le32toh(dc->shift_time_sync0);
shift1 = le32toh(dc->shift_time_sync1);
factor0 = le16toh(dc->cycle_time_sync0_factor);
factor1 = le16toh(dc->cycle_time_sync1_factor);
activate = le16toh(dc->assign_activate) >> 8;
cyclic = le16toh(dc->assign_activate) & 0xff;
if (activate & ESC_DC_ACTIVATION_REGISTER_SYNC_OUT_ACTIVE)
sync_out = true;
else
sync_out = false;
if (activate & ESC_DC_ACTIVATION_REGISTER_SYNC0_ACTIVE)
sync0 = true;
else
sync0 = false;
if (activate & ESC_DC_ACTIVATION_REGISTER_SYNC1_ACTIVE)
sync1 = true;
else
sync1 = false;
if (cyclic & ESC_DC_CYCLIC_UNIT_CONTROL_LATCH0_PDI)
latch0 = true;
else
latch0 = false;
if (cyclic & ESC_DC_CYCLIC_UNIT_CONTROL_LATCH1_PDI)
latch1 = true;
else
latch1 = false;
if (sync_out) {
if (sync0 && !cycle0) {
/* factor and cycle 0? */
if (!factor0)
factor0 = 1;
cycle0 = period_cycle * factor0;
}
if (sync1 && factor1) {
if (period_cycle < cycle0) {
log_send(LOG_T_WARNING,
"Period cycle time is smaller than sync0 cycle time. %dns < %dns",
period_cycle, cycle0);
}
if (factor1 < 0) {
shift1 = (period_cycle * -factor1);
shift1 -= cycle0;
} else {
shift1 = cycle0 * (factor1 - 1) + shift1;
}
}
}
esc_esi_category_strings_get(dev->categories,
dc->dc_sync_name, string, 200);
log_send(LOG_T_DEBUG, "\tDC: %s", string);
log_send(LOG_T_DEBUG, "\t\tSYNC0: cycle: %d, factor: %d, shift: %d",
cycle0, factor0, shift0);
log_send(LOG_T_DEBUG, "\t\tSYNC1: factor: %d, shift: %d",
factor1, shift1);
log_send(LOG_T_DEBUG, "\t\tassign_activate: 0x%04x",
le16toh(dc->assign_activate));
dev->dc_sync.sync0_cycle = cycle0;
dev->dc_sync.sync0_shift = shift0;
dev->dc_sync.sync1_shift = shift1;
dev->dc_sync.sync_out_active = sync_out;
dev->dc_sync.sync0_active = sync0;
dev->dc_sync.sync1_active = sync1;
dev->dc_sync.latch0_pdi = latch0;
dev->dc_sync.latch1_pdi = latch1;
dc++;
}
return 0;
}
static int esc_esi_device_fill_sm(struct esc_device *dev)
{
struct esc_esi_category *cat;
struct esc_esi_syncmanager *sm;
int i, nr;
for (cat = dev->categories;
cat && cat->type != ESC_ESI_CATEGORY_SYNCMANAGER;
cat = cat->next);
if (!cat)
return -1;
if (cat->length % sizeof(struct esc_esi_syncmanager)) {
log_send(LOG_T_ERROR,
"SII syncmanager data is not a multiple of %zd",
sizeof(struct esc_esi_syncmanager));
return -1;
}
nr = cat->length / sizeof(struct esc_esi_syncmanager);
if (nr > ESC_SYNCMANAGER_MAX) {
log_send(LOG_T_ERROR,
"SII syncmanager data contains more than %d entries: %d",
ESC_SYNCMANAGER_MAX, nr);
return -1;
}
sm = (struct esc_esi_syncmanager *)cat->data;
for (i = 0; i < nr; i++) {
dev->sm[i].start = le16toh(sm[i].offset);
dev->sm[i].len = le16toh(sm[i].size);
dev->sm[i].sm = i;
dev->sm[i].enabled = sm[i].enabled;
log_send(LOG_T_DEBUG,
"\tsm%d: %zd bytes @ 0x%zx, enabled: %d, ctrl: %02x",
i, dev->sm[i].len, dev->sm[i].start, dev->sm[i].enabled,
sm[i].ctrl);
if (sm[i].ctrl == ESC_SYNCMANAGER_CONTROL_INT_PDI) {
dev->rx_pdo = true;
dev->rx_pdo_sm = i;
log_send(LOG_T_DEBUG, "\tUsing sm%d for rx pdo",
dev->rx_pdo_sm);
}
if (sm[i].ctrl & ESC_SYNCMANAGER_CONTROL_WRITE) {
dev->tx_pdo = true;
dev->tx_pdo_sm = i;
dev->tx_pdo_watchdog =
sm[i].ctrl & ESC_SYNCMANAGER_CONTROL_WATCHDOG_ENABLE;
log_send(LOG_T_DEBUG, "\tUsing sm%d for tx pdo",
dev->tx_pdo_sm);
}
}
return 0;
}
static int esc_esi_device_fill_pdo_cat(struct esc_device *dev,
struct esc_esi_category *cat)
{
struct esc_esi_pdo *pdo;
struct esc_esi_pdo_entry *pdo_entry;
int i, j, nr, entries;
if (cat->length % sizeof(struct esc_esi_pdo)) {
log_send(LOG_T_ERROR,
"SII pdo data is not a multiple of %zd",
sizeof(struct esc_esi_pdo));
return -1;
}
nr = cat->length / sizeof(struct esc_esi_pdo);
for (i = 0; i < nr; i++) {
char string[200];
pdo = ((struct esc_esi_pdo *)cat->data) + i;
entries = pdo->entries;
esc_esi_category_strings_get(dev->categories,
pdo->string, string, 200);
log_send(LOG_T_DEBUG,
"\tPDO index: 0x%04x, sm: %d, entries: %d, name: %s, type: %s",
le16toh(pdo->index), pdo->sm, entries, string,
cat->type == ESC_ESI_CATEGORY_PDO_RX ? "RX" : "TX");
for (j = 0; j < entries && j + i < nr; j++) {
pdo_entry = (struct esc_esi_pdo_entry *)(pdo+1+j);
esc_esi_category_strings_get(dev->categories,
pdo_entry->string, string, 200);
log_send(LOG_T_DEBUG,
"\t\tindex: 0x%04x, bitsize: %d, name: %s",
le16toh(pdo_entry->index), pdo_entry->bitsize,
string);
if (pdo->sm < ESC_SYNCMANAGER_MAX) {
dev->sm[pdo->sm].bitlen += pdo_entry->bitsize;
if (cat->type == ESC_ESI_CATEGORY_PDO_RX) {
dev->rx_pdo_sm = pdo->sm;
dev->rx_pdo = true;
log_send(LOG_T_DEBUG, "\tUsing sm%d for rx pdo",
dev->rx_pdo_sm);
}
if (cat->type == ESC_ESI_CATEGORY_PDO_TX) {
dev->tx_pdo_sm = pdo->sm;
dev->tx_pdo = true;
log_send(LOG_T_DEBUG, "\tUsing sm%d for tx pdo",
dev->tx_pdo_sm);
}
}
}
i += j;
}
for (i = 0; i < ESC_SYNCMANAGER_MAX; i++) {
if (!dev->sm[i].len) {
dev->sm[i].len = dev->sm[i].bitlen / 8;
}
}
return 0;
}
static int esc_esi_device_fill_pdo(struct esc_device *dev)
{
struct esc_esi_category *cat;
int r = 0;
for (cat = dev->categories; cat; cat = cat->next) {
if (cat->type == ESC_ESI_CATEGORY_PDO_RX ||
cat->type == ESC_ESI_CATEGORY_PDO_TX) {
r = esc_esi_device_fill_pdo_cat(dev, cat);
}
}
return r;
}
/*
This function will create a device structure and will initialize it
according to the ESI data found on the device.
*/
int esc_esi_device_fill(struct esc_device *dev)
{
int r = 0;
size_t mbox_rx_offset, mbox_tx_offset;
size_t mbox_rx_size, mbox_tx_size;
int i;
log_send(LOG_T_DEBUG, "%s: Filling device based on ESI information",
dev->name);
dev->categories = esc_esi_categories_create(dev->ec, &dev->addr);
esc_esi_device_fill_sm(dev);
esc_esi_device_fill_pdo(dev);
mbox_rx_offset = esc_esi_standard_rx_mailbox_offset_get(dev->ec, &dev->addr);
mbox_rx_size = esc_esi_standard_rx_mailbox_size_get(dev->ec, &dev->addr);
mbox_tx_offset = esc_esi_standard_tx_mailbox_offset_get(dev->ec, &dev->addr);
mbox_tx_size = esc_esi_standard_tx_mailbox_size_get(dev->ec, &dev->addr);
for (i = 0; i < ESC_SYNCMANAGER_MAX; i++) {
if (!dev->sm[i].enabled)
continue;
if (dev->sm[i].start == mbox_rx_offset) {
if (!dev->sm[i].len)
dev->sm[i].len = mbox_rx_size;
dev->mbox_wr_sm = i;
}
if (dev->sm[i].start == mbox_tx_offset) {
if (!dev->sm[i].len)
dev->sm[i].len = mbox_tx_size;
dev->mbox_rd_sm = i;
}
}
esc_esi_device_fill_dc(dev);
return r;
}