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

#define DEBUG 0

    31

    32

#if DEBUG

    33

#include <iostream>

    34

#endif

    35

    36

#include <iomanip>

    37

#include <sstream>

    38

using namespace std;

    39

    40

#include "DataTypeHandler.h"

    41

    42

#include "ecrt.h"

    43

    44

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

    45

    46

DataTypeHandler::DataTypeHandler()

    47

{

    48

}

    49

    50

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

    51

    52

const DataTypeHandler::DataType *DataTypeHandler::findDataType(

    53

        const string &str

    54

        )

    55

{

    56

    const DataType *d;

    57

    58

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

    59

        if (str == d->name)

    60

            return d;

    61

    62

    return NULL; // FIXME exception

    63

}

    64

    65

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

    66

    67

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

    68

{

    69

    const DataType *d;

    70

    71

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

    72

        if (code == d->code)

    73

            return d;

    74

    75

    return NULL;

    76

}

    77

    78

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

    79

    80

size_t DataTypeHandler::interpretAsType(

    81

        const DataType *type,

    82

        const string &source,

    83

        void *target,

    84

        size_t targetSize

    85

        )

    86

{ 

    87

    stringstream str;

    88

    size_t dataSize = type->byteSize;

    89

    90

#if DEBUG

    91

	cerr << __func__ << "(targetSize=" << targetSize << ")" << endl;

    92

#endif

    93

    94

    str << source;

    95

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

    96

    str.exceptions(ios::failbit);

    97

    98

#if DEBUG

    99

	cerr << "code=" << type->code << endl;

   100

#endif

   101

   102

    switch (type->code) {

   103

        case 0x0002: // int8

   104

            {

   105

                int16_t val; // uint8_t is interpreted as char

   106

                str >> val;

   107

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

   108

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

   109

                *(uint8_t *) target = val;

   110

                break;

   111

            }

   112

        case 0x0003: // int16

   113

            {

   114

                int16_t val;

   115

                str >> val;

   116

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

   117

                break;

   118

            }

   119

        case 0x0004: // int32

   120

            {

   121

                int32_t val;

   122

                str >> val;

   123

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

   124

                break;

   125

            }

   126

        case 0x0005: // uint8

   127

            {

   128

                uint16_t val; // uint8_t is interpreted as char

   129

                str >> val;

   130

                if (val > 0xff)

   131

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

   132

                *(uint8_t *) target = val;

   133

                break;

   134

            }

   135

        case 0x0006: // uint16

   136

            {

   137

                uint16_t val;

   138

                str >> val;

   139

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

   140

                break;

   141

            }

   142

        case 0x0007: // uint32

   143

            {

   144

                uint32_t val;

   145

                str >> val;

   146

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

   147

                break;

   148

            }

   149

        case 0x0009: // string

   150

        case 0x000a: // octet_string

   151

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

   152

            if (dataSize >= targetSize) {

   153

                stringstream err;

   154

                err << "String too large";

   155

                throw SizeException(err.str());

   156

            }

   157

            str >> (char *) target;

   158

            break;

   159

   160

        default:

   161

            {

   162

                stringstream err;

   163

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

   164

                throw runtime_error(err.str());

   165

            }

   166

    }

   167

   168

#if DEBUG

   169

	printRawData(cerr, (const uint8_t *) target, dataSize);

   170

#endif

   171

   172

    return dataSize;

   173

}

   174

   175

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

   176

   177

void DataTypeHandler::outputData(

   178

        ostream &o,

   179

        const DataType *type,

   180

        void *data,

   181

        size_t dataSize

   182

        )

   183

{ 

   184

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

   185

        stringstream err;

   186

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

   187

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

   188

            << dataSize << " byte.";

   189

        throw SizeException(err.str());

   190

    }

   191

   192

    o << setfill('0');

   193

   194

    switch (type->code) {

   195

        case 0x0002: // int8

   196

            {

   197

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

   198

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

   199

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

   200

            }

   201

            break;

   202

        case 0x0003: // int16

   203

            {

   204

                int16_t val = le16_to_cpup(data);

   205

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

   206

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

   207

            }

   208

            break;

   209

        case 0x0004: // int32

   210

            {

   211

                int32_t val = le32_to_cpup(data);

   212

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

   213

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

   214

            }

   215

            break;

   216

        case 0x0005: // uint8

   217

            {

   218

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

   219

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

   220

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

   221

            }

   222

            break;

   223

        case 0x0006: // uint16

   224

            {

   225

                uint16_t val = le16_to_cpup(data);

   226

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

   227

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

   228

            }

   229

            break;

   230

        case 0x0007: // uint32

   231

            {

   232

                uint32_t val = le32_to_cpup(data);

   233

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

   234

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

   235

            }

   236

            break;

   237

        case 0x0009: // string

   238

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

   239

            break;

   240

        case 0x000a: // octet_string

   241

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

   242

            break;

   243

        default:

   244

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

   245

            break;

   246

    }

   247

}

   248

   249

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

   250

   251

void DataTypeHandler::printRawData(

   252

        ostream &o,

   253

        const uint8_t *data,

   254

        size_t size

   255

        )

   256

{

   257

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

   258

    while (size--) {

   259

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

   260

        if (size)

   261

            o << " ";

   262

    }

   263

    o << endl;

   264

}

   265

   266

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

   267

   268

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

   269

    {"int8",         0x0002, 1},

   270

    {"int16",        0x0003, 2},

   271

    {"int32",        0x0004, 4},

   272

    {"uint8",        0x0005, 1},

   273

    {"uint16",       0x0006, 2},

   274

    {"uint32",       0x0007, 4},

   275

    {"string",       0x0009, 0},

   276

    {"octet_string", 0x000a, 0},

   277

    {"raw",          0xffff, 0},

   278

    //{"int64",        8},

   279

    //{"uint64",       8},

   280

    {}

   281

};

   282

   283

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