1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006-2009  Florian Pose, Ingenieurgemeinschaft IgH

     6

 *

     7

 *  This file is part of the IgH EtherCAT Master.

     8

 *

     9

 *  The IgH EtherCAT Master is free software; you can redistribute it and/or

    10

 *  modify it under the terms of the GNU General Public License version 2, as

    11

 *  published by the Free Software Foundation.

    12

 *

    13

 *  The IgH EtherCAT Master is distributed in the hope that it will be useful,

    14

 *  but WITHOUT ANY WARRANTY; without even the implied warranty of

    15

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General

    16

 *  Public License for more details.

    17

 *

    18

 *  You should have received a copy of the GNU General Public License along

    19

 *  with the IgH EtherCAT Master; if not, write to the Free Software

    20

 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

    21

 *

    22

 *  ---

    23

 *

    24

 *  The license mentioned above concerns the source code only. Using the

    25

 *  EtherCAT technology and brand is only permitted in compliance with the

    26

 *  industrial property and similar rights of Beckhoff Automation GmbH.

    27

 *

    28

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

    29

    30

#include <iomanip>

    31

#include <sstream>

    32

using namespace std;

    33

    34

#include "DataTypeHandler.h"

    35

    36

#include "ecrt.h"

    37

    38

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

    39

    40

DataTypeHandler::DataTypeHandler()

    41

{

    42

}

    43

    44

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

    45

    46

const DataTypeHandler::DataType *DataTypeHandler::findDataType(

    47

        const string &str

    48

        )

    49

{

    50

    const DataType *d;

    51

    52

    for (d = dataTypes; d->name; d++)

    53

        if (str == d->name)

    54

            return d;

    55

    56

    return NULL; // FIXME exception

    57

}

    58

    59

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

    60

    61

const DataTypeHandler::DataType *DataTypeHandler::findDataType(uint16_t code)

    62

{

    63

    const DataType *d;

    64

    65

    for (d = dataTypes; d->name; d++)

    66

        if (code == d->code)

    67

            return d;

    68

    69

    return NULL;

    70

}

    71

    72

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

    73

    74

size_t DataTypeHandler::interpretAsType(

    75

        const DataType *type,

    76

        const string &source,

    77

        void *target,

    78

        size_t targetSize

    79

        )

    80

{ 

    81

    stringstream str;

    82

    size_t dataSize = type->byteSize;

    83

    84

    str << source;

    85

    str >> resetiosflags(ios::basefield); // guess base from prefix

    86

    str.exceptions(ios::failbit);

    87

    88

    switch (type->code) {

    89

        case 0x0002: // int8

    90

            {

    91

                int16_t val; // uint8_t is interpreted as char

    92

                str >> val;

    93

                if (val > 127 || val < -128)

    94

                    throw ios::failure("Value out of range");

    95

                *(uint8_t *) target = val;

    96

                break;

    97

            }

    98

        case 0x0003: // int16

    99

            {

   100

                int16_t val;

   101

                str >> val;

   102

                *(int16_t *) target = cpu_to_le16(val);

   103

                break;

   104

            }

   105

        case 0x0004: // int32

   106

            {

   107

                int32_t val;

   108

                str >> val;

   109

                *(int32_t *) target = cpu_to_le32(val);

   110

                break;

   111

            }

   112

        case 0x0005: // uint8

   113

            {

   114

                uint16_t val; // uint8_t is interpreted as char

   115

                str >> val;

   116

                if (val > 0xff)

   117

                    throw ios::failure("Value out of range");

   118

                *(uint8_t *) target = val;

   119

                break;

   120

            }

   121

        case 0x0006: // uint16

   122

            {

   123

                uint16_t val;

   124

                str >> val;

   125

                *(uint16_t *) target = cpu_to_le16(val);

   126

                break;

   127

            }

   128

        case 0x0007: // uint32

   129

            {

   130

                uint32_t val;

   131

                str >> val;

   132

                *(uint32_t *) target = cpu_to_le32(val);

   133

                break;

   134

            }

   135

        case 0x0009: // string

   136

        case 0x000a: // octet_string

   137

            dataSize = str.str().size();

   138

            if (dataSize >= targetSize) {

   139

                stringstream err;

   140

                err << "String too large";

   141

                throw SizeException(err.str());

   142

            }

   143

            str >> (char *) target;

   144

            break;

   145

   146

        default:

   147

            {

   148

                stringstream err;

   149

                err << "Unknown data type 0x" << hex << type->code;

   150

                throw runtime_error(err.str());

   151

            }

   152

    }

   153

   154

    return dataSize;

   155

}

   156

   157

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

   158

   159

void DataTypeHandler::outputData(

   160

        ostream &o,

   161

        const DataType *type,

   162

        void *data,

   163

        size_t dataSize

   164

        )

   165

{ 

   166

    if (type->byteSize && dataSize != type->byteSize) {

   167

        stringstream err;

   168

        err << "Data type mismatch. Expected " << type->name

   169

            << " with " << type->byteSize << " byte, but got "

   170

            << dataSize << " byte.";

   171

        throw SizeException(err.str());

   172

    }

   173

   174

    o << setfill('0');

   175

   176

    switch (type->code) {

   177

        case 0x0002: // int8

   178

            {

   179

                int val = (int) *(int8_t *) data;

   180

                o << "0x" << hex << setw(2) << val

   181

                    << " " << dec << val << endl;

   182

            }

   183

            break;

   184

        case 0x0003: // int16

   185

            {

   186

                int16_t val = le16_to_cpup(data);

   187

                o << "0x" << hex << setw(4) << val

   188

                    << " " << dec << val << endl;

   189

            }

   190

            break;

   191

        case 0x0004: // int32

   192

            {

   193

                int32_t val = le32_to_cpup(data);

   194

                o << "0x" << hex << setw(8) << val

   195

                    << " " << dec << val << endl;

   196

            }

   197

            break;

   198

        case 0x0005: // uint8

   199

            {

   200

                unsigned int val = (unsigned int) *(uint8_t *) data;

   201

                o << "0x" << hex << setw(2) << val

   202

                    << " " << dec << val << endl;

   203

            }

   204

            break;

   205

        case 0x0006: // uint16

   206

            {

   207

                uint16_t val = le16_to_cpup(data);

   208

                o << "0x" << hex << setw(4) << val

   209

                    << " " << dec << val << endl;

   210

            }

   211

            break;

   212

        case 0x0007: // uint32

   213

            {

   214

                uint32_t val = le32_to_cpup(data);

   215

                o << "0x" << hex << setw(8) << val

   216

                    << " " << dec << val << endl;

   217

            }

   218

            break;

   219

        case 0x0009: // string

   220

            o << string((const char *) data, dataSize) << endl;

   221

            break;

   222

        case 0x000a: // octet_string

   223

            o << string((const char *) data, dataSize) << endl;

   224

            break;

   225

        default:

   226

            printRawData(o, (const uint8_t *) data, dataSize); // FIXME

   227

            break;

   228

    }

   229

}

   230

   231

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

   232

   233

void DataTypeHandler::printRawData(

   234

        ostream &o,

   235

        const uint8_t *data,

   236

        size_t size

   237

        )

   238

{

   239

    o << hex << setfill('0');

   240

    while (size--) {

   241

        o << "0x" << setw(2) << (unsigned int) *data++;

   242

        if (size)

   243

            o << " ";

   244

    }

   245

    o << endl;

   246

}

   247

   248

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

   249

   250

const DataTypeHandler::DataType DataTypeHandler::dataTypes[] = {

   251

    {"int8",         0x0002, 1},

   252

    {"int16",        0x0003, 2},

   253

    {"int32",        0x0004, 4},

   254

    {"uint8",        0x0005, 1},

   255

    {"uint16",       0x0006, 2},

   256

    {"uint32",       0x0007, 4},

   257

    {"string",       0x0009, 0},

   258

    {"octet_string", 0x000a, 0},

   259

    {"raw",          0xffff, 0},

   260

    //{"int64",        8},

   261

    //{"uint64",       8},

   262

    {}

   263

};

   264

   265

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