1

/*****************************************************************************

     2

 *

     3

 * $Id$

     4

 *

     5

 ****************************************************************************/

     6

     7

#include <sys/types.h>

     8

#include <sys/stat.h>

     9

#include <fcntl.h>

    10

#include <errno.h>

    11

#include <sys/ioctl.h>

    12

    13

#include <iostream>

    14

#include <iomanip>

    15

#include <sstream>

    16

using namespace std;

    17

    18

#include "Master.h"

    19

#include "../master/ioctl.h"

    20

    21

/****************************************************************************/

    22

    23

Master::Master()

    24

{

    25

    index = 0;

    26

    fd = -1;

    27

}

    28

    29

/****************************************************************************/

    30

    31

Master::~Master()

    32

{

    33

    close();

    34

}

    35

    36

/****************************************************************************/

    37

    38

void Master::open(unsigned int index)

    39

{

    40

    stringstream deviceName;

    41

    42

    Master::index = index;

    43

    44

    deviceName << "/dev/EtherCAT" << index;

    45

    46

    if ((fd = ::open(deviceName.str().c_str(), O_RDONLY)) == -1) {

    47

        stringstream err;

    48

        err << "Failed to open master device " << deviceName.str() << ": "

    49

            << strerror(errno);

    50

        throw MasterException(err.str());

    51

    }

    52

}

    53

    54

/****************************************************************************/

    55

    56

void Master::close()

    57

{

    58

    if (fd == -1)

    59

        return;

    60

    61

    ::close(fd);

    62

}

    63

    64

/****************************************************************************/

    65

    66

unsigned int Master::slaveCount()

    67

{

    68

    unsigned int numSlaves;

    69

    70

    if (ioctl(fd, EC_IOCTL_SLAVE_COUNT, &numSlaves)) {

    71

        stringstream err;

    72

        err << "Failed to get number of slaves: " << strerror(errno);

    73

        throw MasterException(err.str());

    74

    }

    75

    76

    return numSlaves;

    77

}

    78

    79

/****************************************************************************/

    80

    81

void Master::listSlaves()

    82

{

    83

    unsigned int numSlaves = slaveCount(), i;

    84

    struct ec_ioctl_slave_info *infos, *info;

    85

    uint16_t lastAlias, aliasIndex;

    86

    87

    if (!numSlaves)

    88

        return;

    89

    90

    infos = new struct ec_ioctl_slave_info[numSlaves];

    91

    92

    if (ioctl(fd, EC_IOCTL_SLAVE_INFO, infos)) {

    93

        stringstream err;

    94

        err << "Failed to get slave information: " << strerror(errno);

    95

        throw MasterException(err.str());

    96

    }

    97

    98

    lastAlias = 0;

    99

    aliasIndex = 0;

   100

    for (i = 0; i < numSlaves; i++) {

   101

        info = &infos[i];

   102

        cout << setw(2) << info->ring_position << "  ";

   103

   104

        if (info->alias) {

   105

            lastAlias = info->alias;

   106

            aliasIndex = 0;

   107

        }

   108

        if (lastAlias) {

   109

            cout << setw(10) << "#" << lastAlias << ":" << aliasIndex;

   110

        }

   111

   112

        cout << "  " << slaveState(info->state) << "  ";

   113

   114

        if (strlen(info->description)) {

   115

            cout << info->description;

   116

        } else {

   117

            cout << "0x" << hex << setfill('0') << info->vendor_id

   118

                << ":0x" << info->product_code;

   119

        }

   120

   121

        cout << endl;

   122

    }

   123

   124

    delete [] infos;

   125

}

   126

   127

/****************************************************************************/

   128

   129

string Master::slaveState(uint8_t state) const

   130

{

   131

    switch (state) {

   132

        case 1: return "INIT";

   133

        case 2: return "PREOP";

   134

        case 4: return "SAFEOP";

   135

        case 8: return "OP";

   136

        default: return "???";

   137

    }

   138

}

   139

   140

/****************************************************************************/