1

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

     2

 *

     3

 *  m a s t e r . c

     4

 *

     5

 *  Methoden für einen EtherCAT-Master.

     6

 *

     7

 *  $Id$

     8

 *

     9

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

    10

    11

#include <linux/module.h>

    12

#include <linux/kernel.h>

    13

#include <linux/string.h>

    14

#include <linux/slab.h>

    15

#include <linux/delay.h>

    16

    18

#include "globals.h"

    19

#include "master.h"

    20

#include "slave.h"

    21

#include "types.h"

    22

#include "device.h"

    23

#include "command.h"

    24

    25

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

    26

    27

/**

    28

   Konstruktor des EtherCAT-Masters.

    29

*/

    30

    31

void ec_master_init(ec_master_t *master /**< EtherCAT-Master */)

    32

{

    33

    master->slaves = NULL;

    34

    master->device = NULL;

    35

    36

    INIT_LIST_HEAD(&master->commands);

    37

    INIT_LIST_HEAD(&master->domains);

    38

    39

    ec_master_reset(master);

    40

}

    41

    42

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

    43

    44

/**

    45

   Destruktor eines EtherCAT-Masters.

    46

    47

   Entfernt alle Kommandos aus der Liste, löscht den Zeiger

    48

   auf das Slave-Array und gibt die Prozessdaten frei.

    49

*/

    50

    51

void ec_master_clear(ec_master_t *master /**< EtherCAT-Master */)

    52

{

    53

    ec_master_reset(master);

    54

    55

    if (master->device) {

    56

        ec_device_clear(master->device);

    57

        kfree(master->device);

    58

    }

    59

}

    60

    61

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

    62

    63

/**

    64

   Setzt den Master zurück in den Ausgangszustand.

    65

    66

   Bei einem "release" sollte immer diese Funktion aufgerufen werden,

    67

   da sonst Slave-Liste, Domains, etc. weiter existieren.

    68

*/

    69

    70

void ec_master_reset(ec_master_t *master

    71

                     /**< Zeiger auf den zurückzusetzenden Master */

    72

                     )

    73

{

    74

    unsigned int i;

    75

    ec_command_t *command, *next_command;

    76

    ec_domain_t *domain, *next_domain;

    77

    78

    // Alle Slaves entfernen

    79

    if (master->slaves) {

    80

        for (i = 0; i < master->slave_count; i++) {

    81

            ec_slave_clear(master->slaves + i);

    82

        }

    83

        kfree(master->slaves);

    84

        master->slaves = NULL;

    85

    }

    86

    master->slave_count = 0;

    87

    88

    // Kommando-Warteschlange leeren

    89

    list_for_each_entry_safe(command, next_command, &master->commands, list) {

    90

        command->state = EC_CMD_ERROR;

    91

        list_del_init(&command->list);

    92

    }

    93

    94

    // Domain-Liste leeren

    95

    list_for_each_entry_safe(domain, next_domain, &master->domains, list) {

    96

        list_del(&domain->list);

    97

        ec_domain_clear(domain);

    98

        kfree(domain);

    99

    }

   100

   101

    master->command_index = 0;

   102

    master->debug_level = 0;

   103

    master->stats.timeouts = 0;

   104

    master->stats.delayed = 0;

   105

    master->stats.corrupted = 0;

   106

    master->stats.unmatched = 0;

   107

    master->stats.t_last = 0;

   108

}

   109

   110

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

   111

   112

/**

   113

   Öffnet das EtherCAT-Geraet des Masters.

   114

   115

   \return 0 wenn alles ok, < 0 wenn kein Gerät registriert wurde oder

   116

           es nicht geoeffnet werden konnte.

   117

*/

   118

   119

int ec_master_open(ec_master_t *master /**< Der EtherCAT-Master */)

   120

{

   121

    if (!master->device) {

   122

        EC_ERR("No device registered!\n");

   123

        return -1;

   124

    }

   125

   126

    if (ec_device_open(master->device)) {

   127

        EC_ERR("Could not open device!\n");

   128

        return -1;

   129

    }

   130

   131

    return 0;

   132

}

   133

   134

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

   135

   136

/**

   137

   Schliesst das EtherCAT-Geraet, auf dem der Master arbeitet.

   138

*/

   139

   140

void ec_master_close(ec_master_t *master /**< EtherCAT-Master */)

   141

{

   142

    if (!master->device) {

   143

        EC_WARN("Warning - Trying to close an unregistered device!\n");

   144

        return;

   145

    }

   146

   147

    if (ec_device_close(master->device))

   148

        EC_WARN("Warning - Could not close device!\n");

   149

}

   150

   151

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

   152

   153

/**

   154

   Stellt ein Kommando in die Warteschlange.

   155

*/

   156

   157

void ec_master_queue_command(ec_master_t *master, /**< EtherCAT-Master */

   158

                             ec_command_t *command /**< Kommando */

   159

                             )

   160

{

   161

    ec_command_t *queued_command;

   162

   163

    // Ist das Kommando schon in der Warteschlange?

   164

    list_for_each_entry(queued_command, &master->commands, list) {

   165

        if (queued_command == command) {

   166

            command->state = EC_CMD_QUEUED;

   167

            if (unlikely(master->debug_level))

   168

                EC_WARN("command already queued.\n");

   169

            return;

   170

        }

   171

    }

   172

   173

    list_add_tail(&command->list, &master->commands);

   174

    command->state = EC_CMD_QUEUED;

   175

}

   176

   177

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

   178

   179

/**

   180

   Sendet die Kommandos in der Warteschlange.

   181

   182

   \return 0 bei Erfolg, sonst < 0

   183

*/

   184

   185

void ec_master_send_commands(ec_master_t *master /**< EtherCAT-Master */)

   186

{

   187

    ec_command_t *command;

   188

    size_t command_size;

   189

    uint8_t *frame_data, *cur_data;

   190

    void *follows_word;

   191

    cycles_t start = 0, end;

   192

   193

    if (unlikely(master->debug_level > 0)) {

   194

        EC_DBG("ec_master_send\n");

   195

        start = get_cycles();

   196

    }

   197

   198

    // Zeiger auf Socket-Buffer holen

   199

    frame_data = ec_device_tx_data(master->device);

   200

    cur_data = frame_data + EC_FRAME_HEADER_SIZE;

   201

    follows_word = NULL;

   202

   203

    // Aktuellen Frame mit Kommandos füllen

   204

    list_for_each_entry(command, &master->commands, list) {

   205

        if (command->state != EC_CMD_QUEUED) continue;

   206

   207

        // Passt das aktuelle Kommando noch in den aktuellen Rahmen?

   208

        command_size = EC_COMMAND_HEADER_SIZE + command->data_size

   209

            + EC_COMMAND_FOOTER_SIZE;

   210

        if (cur_data - frame_data + command_size > EC_MAX_FRAME_SIZE) break;

   211

   212

        command->state = EC_CMD_SENT;

   213

        command->index = master->command_index++;

   214

   215

        if (unlikely(master->debug_level > 0))

   216

            EC_DBG("adding command 0x%02X\n", command->index);

   217

   218

        // Command-Following-Flag im letzten Kommando setzen

   219

        if (follows_word)

   220

            EC_WRITE_U16(follows_word, EC_READ_U16(follows_word) | 0x8000);

   221

   222

        // EtherCAT command header

   223

        EC_WRITE_U8 (cur_data,     command->type);

   224

        EC_WRITE_U8 (cur_data + 1, command->index);

   225

        EC_WRITE_U32(cur_data + 2, command->address.logical);

   226

        EC_WRITE_U16(cur_data + 6, command->data_size & 0x7FF);

   227

        EC_WRITE_U16(cur_data + 8, 0x0000);

   228

        follows_word = cur_data + 6;

   229

        cur_data += EC_COMMAND_HEADER_SIZE;

   230

   231

        // EtherCAT command data

   232

        memcpy(cur_data, command->data, command->data_size);

   233

        cur_data += command->data_size;

   234

   235

        // EtherCAT command footer

   236

        EC_WRITE_U16(cur_data, command->working_counter);

   237

        cur_data += EC_COMMAND_FOOTER_SIZE;

   238

    }

   239

   240

    if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {

   241

        if (unlikely(master->debug_level > 0)) EC_DBG("nothing to send.\n");

   242

        return;

   243

    }

   244

   245

    // EtherCAT frame header

   246

    EC_WRITE_U16(frame_data, ((cur_data - frame_data

   247

                               - EC_FRAME_HEADER_SIZE) & 0x7FF) | 0x1000);

   248

   249

    // Rahmen auffüllen

   250

    while (cur_data - frame_data < EC_MIN_FRAME_SIZE)

   251

        EC_WRITE_U8(cur_data++, 0x00);

   252

   253

    if (unlikely(master->debug_level > 0))

   254

        EC_DBG("Frame size: %i\n", cur_data - frame_data);

   255

   256

    // Send frame

   257

    ec_device_send(master->device, cur_data - frame_data);

   258

   259

    if (unlikely(master->debug_level > 0)) {

   260

        end = get_cycles();

   261

        EC_DBG("ec_master_send finished in %ius.\n",

   262

               (u32) (end - start) * 1000 / cpu_khz);

   263

    }

   264

}

   265

   266

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

   267

   268

/**

   269

   Wertet einen empfangenen Rahmen aus.

   270

   271

   \return 0 bei Erfolg, sonst < 0

   272

*/

   273

   274

void ec_master_receive(ec_master_t *master, /**< EtherCAT-Master */

   275

                      const uint8_t *frame_data, /**< Empfangene Daten */

   276

                      size_t size /**< Anzahl empfangene Datenbytes */

   277

                      )

   278

{

   279

    size_t frame_size, data_size;

   280

    uint8_t command_type, command_index;

   281

    unsigned int cmd_follows, matched;

   282

    const uint8_t *cur_data;

   283

    ec_command_t *command;

   284

   285

    if (unlikely(size < EC_FRAME_HEADER_SIZE)) {

   286

        master->stats.corrupted++;

   287

        ec_master_output_stats(master);

   288

        return;

   289

    }

   290

   291

    cur_data = frame_data;

   292

   293

    // Länge des gesamten Frames prüfen

   294

    frame_size = EC_READ_U16(cur_data) & 0x07FF;

   295

    cur_data += EC_FRAME_HEADER_SIZE;

   296

   297

    if (unlikely(frame_size > size)) {

   298

        master->stats.corrupted++;

   299

        ec_master_output_stats(master);

   300

        return;

   301

    }

   302

   303

    cmd_follows = 1;

   304

    while (cmd_follows) {

   305

        // Kommando-Header auswerten

   306

        command_type  = EC_READ_U8 (cur_data);

   307

        command_index = EC_READ_U8 (cur_data + 1);

   308

        data_size     = EC_READ_U16(cur_data + 6) & 0x07FF;

   309

        cmd_follows   = EC_READ_U16(cur_data + 6) & 0x8000;

   310

        cur_data += EC_COMMAND_HEADER_SIZE;

   311

   312

        if (unlikely(cur_data - frame_data

   313

                     + data_size + EC_COMMAND_FOOTER_SIZE > size)) {

   314

            master->stats.corrupted++;

   315

            ec_master_output_stats(master);

   316

            return;

   317

        }

   318

   319

        // Suche passendes Kommando in der Liste

   320

        matched = 0;

   321

        list_for_each_entry(command, &master->commands, list) {

   322

            if (command->state == EC_CMD_SENT

   323

                && command->type == command_type

   324

                && command->index == command_index

   325

                && command->data_size == data_size) {

   326

                matched = 1;

   327

                break;

   328

            }

   329

        }

   330

   331

        // Kein passendes Kommando in der Liste gefunden

   332

        if (!matched) {

   333

            master->stats.unmatched++;

   334

            ec_master_output_stats(master);

   335

            cur_data += data_size + EC_COMMAND_FOOTER_SIZE;

   336

            continue;

   337

        }

   338

   339

        // Empfangene Daten in Kommando-Datenspeicher kopieren

   340

        memcpy(command->data, cur_data, data_size);

   341

        cur_data += data_size;

   342

   343

        // Working-Counter setzen

   344

        command->working_counter = EC_READ_U16(cur_data);

   345

        cur_data += EC_COMMAND_FOOTER_SIZE;

   346

   347

        // Kommando aus der Liste entfernen

   348

        command->state = EC_CMD_RECEIVED;

   349

        list_del_init(&command->list);

   350

    }

   351

}

   352

   353

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

   354

   355

/**

   356

   Sendet ein einzelnes Kommando und wartet auf den Empfang.

   357

   358

   Wenn der Slave nicht antwortet, wird das Kommando

   359

   nochmals gesendet.

   360

   361

   \return 0 bei Erfolg, sonst < 0

   362

*/

   363

   364

int ec_master_simple_io(ec_master_t *master, /**< EtherCAT-Master */

   365

                        ec_command_t *command /**< Kommando */

   366

                        )

   367

{

   368

    unsigned int response_tries_left;

   369

   370

    response_tries_left = 10;

   371

    do

   372

    {

   373

        ec_master_queue_command(master, command);

   375

   376

        if (command->state == EC_CMD_RECEIVED) {

   377

            break;

   378

        }

   379

        else if (command->state == EC_CMD_TIMEOUT) {

   381

            return -1;

   382

        }

   383

        else if (command->state == EC_CMD_ERROR) {

   385

            return -1;

   386

        }

   390

    }

   391

    while (unlikely(!command->working_counter && --response_tries_left));

   392

   393

    if (unlikely(!response_tries_left)) {

   395

        return -1;

   396

    }

   397

   398

    return 0;

   399

}

   400

   401

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

   402

   403

/**

   404

   Durchsucht den EtherCAT-Bus nach Slaves.

   405

   406

   Erstellt ein Array mit allen Slave-Informationen die für den

   407

   weiteren Betrieb notwendig sind.

   408

   409

   \return 0 bei Erfolg, sonst < 0

   410

*/

   411

   412

int ec_master_bus_scan(ec_master_t *master /**< EtherCAT-Master */)

   413

{

   414

    ec_command_t command;

   415

    ec_slave_t *slave;

   416

    ec_slave_ident_t *ident;

   417

    unsigned int i;

   418

    uint8_t data[2];

   419

   420

    if (master->slaves || master->slave_count) {

   421

        EC_ERR("Slave scan already done!\n");

   422

        return -1;

   423

    }

   424

   425

    // Determine number of slaves on bus

   426

    ec_command_init_brd(&command, 0x0000, 4);

   427

    if (unlikely(ec_master_simple_io(master, &command))) return -1;

   428

    master->slave_count = command.working_counter;

   429

    EC_INFO("Found %i slaves on bus.\n", master->slave_count);

   430

   431

    if (!master->slave_count) return 0;

   432

   433

    if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count

   434

                                                  * sizeof(ec_slave_t),

   435

                                                  GFP_KERNEL))) {

   436

        EC_ERR("Could not allocate memory for slaves!\n");

   437

        return -1;

   438

    }

   439

   440

    // Init slaves

   441

    for (i = 0; i < master->slave_count; i++) {

   442

        slave = master->slaves + i;

   443

        ec_slave_init(slave, master);

   444

        slave->ring_position = i;

   445

        slave->station_address = i + 1;

   446

    }

   447

   448

    // For every slave in the list

   449

    for (i = 0; i < master->slave_count; i++)

   450

    {

   451

        slave = master->slaves + i;

   452

   453

        // Write station address

   454

        EC_WRITE_U16(data, slave->station_address);

   455

        ec_command_init_apwr(&command, slave->ring_position,

   456

                             0x0010, sizeof(uint16_t), data);

   457

        if (unlikely(ec_master_simple_io(master, &command))) {

   458

            EC_ERR("Writing station address failed on slave %i!\n", i);

   459

            return -1;

   460

        }

   461

   462

        // Fetch all slave information

   463

        if (ec_slave_fetch(slave)) return -1;

   464

   465

        // Search for identification in "database"

   466

        ident = slave_idents;

   467

        while (ident) {

   468

            if (unlikely(ident->vendor_id == slave->sii_vendor_id

   469

                         && ident->product_code == slave->sii_product_code)) {

   470

                slave->type = ident->type;

   471

                break;

   472

            }

   473

            ident++;

   474

        }

   475

   476

        if (!slave->type)

   477

            EC_WARN("Unknown slave device (vendor 0x%08X, code 0x%08X) at"

   478

                    " position %i.\n", slave->sii_vendor_id,

   479

                    slave->sii_product_code, i);

   480

    }

   481

   482

    return 0;

   483

}

   484

   485

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

   486

   487

/**

   488

   Statistik-Ausgaben während des zyklischen Betriebs.

   489

   490

   Diese Funktion sorgt dafür, dass Statistiken während des zyklischen

   491

   Betriebs bei Bedarf, aber nicht zu oft ausgegeben werden.

   492