1

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

     2

 *

     3

 *  $Id$

     4

 *

     5

 *  Copyright (C) 2006  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

    10

 *  and/or modify it under the terms of the GNU General Public License

    11

 *  as published by the Free Software Foundation; either version 2 of the

    12

 *  License, or (at your option) any later version.

    13

 *

    14

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

    15

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

    16

 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

    17

 *  GNU General Public License for more details.

    18

 *

    19

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

    20

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

    21

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

    22

 *

    23

 *  The right to use EtherCAT Technology is granted and comes free of

    24

 *  charge under condition of compatibility of product made by

    25

 *  Licensee. People intending to distribute/sell products based on the

    26

 *  code, have to sign an agreement to guarantee that products using

    27

 *  software based on IgH EtherCAT master stay compatible with the actual

    28

 *  EtherCAT specification (which are released themselves as an open

    29

 *  standard) as the (only) precondition to have the right to use EtherCAT

    30

 *  Technology, IP and trade marks.

    31

 *

    32

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

    33

    34

/**

    35

   \file

    36

   Canopen-over-EtherCAT functions.

    37

*/

    38

    39

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

    40

    41

#include <linux/module.h>

    42

    43

#include "canopen.h"

    44

    45

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

    46

    47

ssize_t ec_show_sdo_attribute(struct kobject *, struct attribute *, char *);

    48

ssize_t ec_show_sdo_entry_attribute(struct kobject *, struct attribute *,

    49

                                    char *);

    50

void ec_sdo_clear(struct kobject *);

    51

void ec_sdo_entry_clear(struct kobject *);

    52

    53

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

    54

    55

/** \cond */

    56

    57

EC_SYSFS_READ_ATTR(info);

    58

    59

static struct attribute *sdo_def_attrs[] = {

    60

    &attr_info,

    61

    NULL,

    62

};

    63

    64

static struct sysfs_ops sdo_sysfs_ops = {

    65

    .show = &ec_show_sdo_attribute,

    66

    .store = NULL

    67

};

    68

    69

static struct kobj_type ktype_ec_sdo = {

    70

    .release = ec_sdo_clear,

    71

    .sysfs_ops = &sdo_sysfs_ops,

    72

    .default_attrs = sdo_def_attrs

    73

};

    74

    75

static struct attribute *sdo_entry_def_attrs[] = {

    76

    &attr_info,

    77

    NULL,

    78

};

    79

    80

static struct sysfs_ops sdo_entry_sysfs_ops = {

    81

    .show = &ec_show_sdo_entry_attribute,

    82

    .store = NULL

    83

};

    84

    85

static struct kobj_type ktype_ec_sdo_entry = {

    86

    .release = ec_sdo_entry_clear,

    87

    .sysfs_ops = &sdo_entry_sysfs_ops,

    88

    .default_attrs = sdo_entry_def_attrs

    89

};

    90

    91

/** \endcond */

    92

    93

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

    94

    95

/**

    96

   SDO constructor.

    97

*/

    98

    99

int ec_sdo_init(ec_sdo_t *sdo, /**< SDO */

   100

                uint16_t index, /**< SDO index */

   101

                ec_slave_t *slave /**< parent slave */

   102

                )

   103

{

   104

    sdo->index = index;

   105

    sdo->object_code = 0x00;

   106

    sdo->name = NULL;

   107

    sdo->subindices = 0;

   108

    INIT_LIST_HEAD(&sdo->entries);

   109

   110

    // init kobject and add it to the hierarchy

   111

    memset(&sdo->kobj, 0x00, sizeof(struct kobject));

   112

    kobject_init(&sdo->kobj);

   113

    sdo->kobj.ktype = &ktype_ec_sdo;

   116

        EC_ERR("Failed to set kobj name.\n");

   117

        return -1;

   118

    }

   119

   120

    return 0;

   121

}

   122

   123

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

   124

   125

/**

   126

   SDO destructor.

   127

*/

   128

   129

void ec_sdo_clear(struct kobject *kobj /**< SDO's kobject */)

   130

{

   131

    ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj);

   132

    ec_sdo_entry_t *entry, *next;

   133

   134

    // free all entries

   135

    list_for_each_entry_safe(entry, next, &sdo->entries, list) {

   136

        list_del(&entry->list);

   137

        kobject_del(&entry->kobj);

   138

        kobject_put(&entry->kobj);

   139

    }

   140

   141

    if (sdo->name) kfree(sdo->name);

   142

   143

    kfree(sdo);

   144

}

   145

   146

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

   147

   148

ssize_t ec_sdo_info(ec_sdo_t *sdo, /**< SDO */

   149

                    char *buffer /**< target buffer */

   150

                    )

   151

{

   152

    off_t off = 0;

   153

   154

    off += sprintf(buffer + off, "Index: 0x%04X\n", sdo->index);

   155

    off += sprintf(buffer + off, "Name: %s\n", sdo->name ? sdo->name : "");

   156

    off += sprintf(buffer + off, "Subindices: %i\n", sdo->subindices);

   157

   158

    return off;

   159

}

   160

   161

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

   162

   163

ssize_t ec_show_sdo_attribute(struct kobject *kobj, /**< kobject */

   164

                              struct attribute *attr,

   165

                              char *buffer

   166

                              )

   167

{

   168

    ec_sdo_t *sdo = container_of(kobj, ec_sdo_t, kobj);

   169

   170

    if (attr == &attr_info) {

   171

        return ec_sdo_info(sdo, buffer);

   172

    }

   173

   174

    return 0;

   175

}

   176

   177

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

   178

   179

/**

   180

   SDO entry constructor.

   181

*/

   182

   183

int ec_sdo_entry_init(ec_sdo_entry_t *entry, /**< SDO entry */

   184

                      uint8_t subindex, /**< SDO entry subindex */

   185

                      ec_sdo_t *sdo /**< parent SDO */

   186

                      )

   187

{

   188

    entry->subindex = subindex;

   189

    entry->data_type = 0x0000;

   190

    entry->bit_length = 0;

   191

    entry->description = NULL;

   192

   193

    // init kobject and add it to the hierarchy

   194

    memset(&entry->kobj, 0x00, sizeof(struct kobject));

   195

    kobject_init(&entry->kobj);

   196

    entry->kobj.ktype = &ktype_ec_sdo_entry;

   197

    entry->kobj.parent = &sdo->kobj;

   199

        EC_ERR("Failed to set kobj name.\n");

   200

        return -1;

   201

    }

   202

   203

    return 0;

   204

}

   205

   206

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

   207

   208

/**

   209

   SDO destructor.

   210

*/

   211

   212

void ec_sdo_entry_clear(struct kobject *kobj /**< SDO entry's kobject */)

   213

{

   214

    ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj);

   215

   216

    if (entry->description) kfree(entry->description);

   217

   218

    kfree(entry);

   219

}

   220

   221

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

   222

   223

ssize_t ec_sdo_entry_info(ec_sdo_entry_t *entry, /**< SDO entry */

   224

                          char *buffer /**< target buffer */

   225

                          )

   226

{

   227

    off_t off = 0;

   228

   229

    off += sprintf(buffer + off, "Subindex: 0x%02X\n", entry->subindex);

   230

    off += sprintf(buffer + off, "Description: %s\n",

   231

                   entry->description ? entry->description : "");

   232

    off += sprintf(buffer + off, "Data type: 0x%04X\n", entry->data_type);

   233

    off += sprintf(buffer + off, "Bit length: %i\n", entry->bit_length);

   234

   235

    return off;

   236

}

   237

   238

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

   239

   240

ssize_t ec_show_sdo_entry_attribute(struct kobject *kobj, /**< kobject */

   241

                                    struct attribute *attr,

   242

                                    char *buffer

   243

                                    )

   244

{

   245

    ec_sdo_entry_t *entry = container_of(kobj, ec_sdo_entry_t, kobj);

   246

   247

    if (attr == &attr_info) {

   248

        return ec_sdo_entry_info(entry, buffer);

   249

    }

   250

   251

    return 0;

   252

}

   253

   254

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

   255

   256

#if 0

   257

int ecrt_slave_sdo_read(ec_slave_t *slave, /**< EtherCAT slave */

   258

                        uint16_t sdo_index, /**< SDO index */

   259

                        uint8_t sdo_subindex, /**< SDO subindex */

   260

                        uint8_t *target, /**< memory for value */

   261

                        size_t *size /**< target memory size */

   262

                        )

   263

{

   264

    uint8_t *data;

   265

    size_t rec_size, data_size;

   266

    uint32_t complete_size;

   267

    ec_datagram_t datagram;

   268

   269

    ec_datagram_init(&datagram);

   270

   271

    if (!(data = ec_slave_mbox_prepare_send(slave, &datagram, 0x03, 6)))

   272

        goto err;

   273

   274

    EC_WRITE_U16(data, 0x2 << 12); // SDO request

   275

    EC_WRITE_U8 (data + 2, 0x2 << 5); // initiate upload request

   276

    EC_WRITE_U16(data + 3, sdo_index);

   277

    EC_WRITE_U8 (data + 5, sdo_subindex);

   278

   279

    if (!(data = ec_slave_mbox_simple_io(slave, &datagram, &rec_size)))

   280

        goto err;

   281

   282

    if (EC_READ_U16(data) >> 12 == 0x2 && // SDO request

   283

        EC_READ_U8 (data + 2) >> 5 == 0x4) { // abort SDO transfer request

   284

        EC_ERR("SDO upload 0x%04X:%X aborted on slave %i.\n",

   285

               sdo_index, sdo_subindex, slave->ring_position);

   286

        ec_canopen_abort_msg(EC_READ_U32(data + 6));

   287

        goto err;

   288

    }

   289

   290

    if (EC_READ_U16(data) >> 12 != 0x3 || // SDO response

   291

        EC_READ_U8 (data + 2) >> 5 != 0x2 || // initiate upload response

   292

        EC_READ_U16(data + 3) != sdo_index || // index

   293

        EC_READ_U8 (data + 5) != sdo_subindex) { // subindex

   294

        EC_ERR("SDO upload 0x%04X:%X failed:\n", sdo_index, sdo_subindex);

   295

        EC_ERR("Invalid SDO upload response at slave %i!\n",

   296

               slave->ring_position);

   297

        ec_print_data(data, rec_size);

   298

        goto err;

   299

    }

   300

   301

    if (rec_size < 10) {

   302

        EC_ERR("Received currupted SDO upload response!\n");

   303

        ec_print_data(data, rec_size);

   304

        goto err;

   305

    }

   306

   307

    if ((complete_size = EC_READ_U32(data + 6)) > *size) {

   308

        EC_ERR("SDO data does not fit into buffer (%i / %i)!\n",

   309

               complete_size, *size);

   310

        goto err;

   311

    }

   312

   313

    data_size = rec_size - 10;

   314

   315

    if (data_size != complete_size) {

   316

        EC_ERR("SDO data incomplete - Fragmenting not implemented.\n");

   317

        goto err;

   318

    }

   319

   320

    memcpy(target, data + 10, data_size);

   321

   322

    ec_datagram_clear(&datagram);

   323

    return 0;

   324

 err:

   325

    ec_datagram_clear(&datagram);

   326

    return -1;

   327

}

   328

#endif

   329

   330

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