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

    17

#include "../include/ecrt.h"

    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

#include "ethernet.h"

    25

    26

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

    27

    28

/**

    29

   Konstruktor des EtherCAT-Masters.

    30

*/

    31

    32

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

    33

{

    34

    master->slaves = NULL;

    35

    master->device = NULL;

    36

    37

    INIT_LIST_HEAD(&master->command_queue);

    38

    INIT_LIST_HEAD(&master->domains);

    39

    INIT_LIST_HEAD(&master->eoe_slaves);

    40

    41

    ec_command_init(&master->simple_command);

    42

    ec_command_init(&master->watch_command);

    43

    44

    ec_master_reset(master);

    45

}

    46

    47

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

    48

    49

/**

    50

   Destruktor eines EtherCAT-Masters.

    51

    52

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

    53

   auf das Slave-Array und gibt die Prozessdaten frei.

    54

*/

    55

    56

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

    57

{

    58

    ec_master_reset(master);

    59

    60

    if (master->device) {

    61

        ec_device_clear(master->device);

    62

        kfree(master->device);

    63

    }

    64

    65

    ec_command_clear(&master->simple_command);

    66

    ec_command_clear(&master->watch_command);

    67

}

    68

    69

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

    70

    71

/**

    72

   Setzt den Master zurück in den Ausgangszustand.

    73

    74

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

    75

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

    76

*/

    77

    78

void ec_master_reset(ec_master_t *master

    79

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

    80

                     )

    81

{

    82

    unsigned int i;

    83

    ec_command_t *command, *next_c;

    84

    ec_domain_t *domain, *next_d;

    85

    ec_eoe_t *eoe, *next_eoe;

    86

    87

    // Alle Slaves entfernen

    88

    if (master->slaves) {

    89

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

    90

            ec_slave_clear(master->slaves + i);

    91

        kfree(master->slaves);

    92

        master->slaves = NULL;

    93

    }

    94

    master->slave_count = 0;

    95

    96

    // Kommando-Warteschlange leeren

    97

    list_for_each_entry_safe(command, next_c, &master->command_queue, queue) {

    98

        command->state = EC_CMD_ERROR;

    99

        list_del_init(&command->queue);

   100

    }

   101

   102

    // Domain-Liste leeren

   103

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

   104

        list_del(&domain->list);

   105

        ec_domain_clear(domain);

   106

        kfree(domain);

   107

    }

   108

   109

    // EOE-Liste leeren

   110

    list_for_each_entry_safe(eoe, next_eoe, &master->eoe_slaves, list) {

   111

        list_del(&eoe->list);

   112

        ec_eoe_clear(eoe);

   113

        kfree(eoe);

   114

    }

   115

   116

    master->command_index = 0;

   117

    master->debug_level = 0;

   118

    master->timeout = 500; // 500us

   119

   120

    master->slaves_responding = 0;

   121

    master->slave_states = EC_SLAVE_STATE_UNKNOWN;

   122

   123

    master->stats.timeouts = 0;

   124

    master->stats.delayed = 0;

   125

    master->stats.corrupted = 0;

   126

    master->stats.unmatched = 0;

   127

    master->stats.t_last = 0;

   128

}

   129

   130

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

   131

   132

/**

   133

   Stellt ein Kommando in die Warteschlange.

   134

*/

   135

   136

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

   137

                             ec_command_t *command /**< Kommando */

   138

                             )

   139

{

   140

    ec_command_t *queued_command;

   141

   142

    // Ist das Kommando schon in der Warteschlange?

   143

    list_for_each_entry(queued_command, &master->command_queue, queue) {

   144

        if (queued_command == command) {

   145

            command->state = EC_CMD_QUEUED;

   146

            if (unlikely(master->debug_level))

   147

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

   148

            return;

   149

        }

   150

    }

   151

   152

    list_add_tail(&command->queue, &master->command_queue);

   153

    command->state = EC_CMD_QUEUED;

   154

}

   155

   156

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

   157

   158

/**

   159

   Sendet die Kommandos in der Warteschlange.

   160

   161

   \return 0 bei Erfolg, sonst < 0

   162

*/

   163

   164

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

   165

{

   166

    ec_command_t *command;

   167

    size_t command_size;

   168

    uint8_t *frame_data, *cur_data;

   169

    void *follows_word;

   170

    cycles_t start = 0, end;

   171

   172

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

   173

        EC_DBG("ec_master_send\n");

   174

        start = get_cycles();

   175

    }

   176

   177

    // Zeiger auf Socket-Buffer holen

   178

    frame_data = ec_device_tx_data(master->device);

   179

    cur_data = frame_data + EC_FRAME_HEADER_SIZE;

   180

    follows_word = NULL;

   181

   182

    // Aktuellen Frame mit Kommandos füllen

   183

    list_for_each_entry(command, &master->command_queue, queue) {

   184

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

   185

   186

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

   187

        command_size = EC_COMMAND_HEADER_SIZE + command->data_size

   188

            + EC_COMMAND_FOOTER_SIZE;

   189

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

   190

   191

        command->state = EC_CMD_SENT;

   192

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

   193

   194

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

   195

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

   196

   197

        // Command-Following-Flag im letzten Kommando setzen

   198

        if (follows_word)

   199

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

   200

   201

        // EtherCAT command header

   202

        EC_WRITE_U8 (cur_data,     command->type);

   203

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

   204

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

   205

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

   206

        EC_WRITE_U16(cur_data + 8, 0x0000);

   207

        follows_word = cur_data + 6;

   208

        cur_data += EC_COMMAND_HEADER_SIZE;

   209

   210

        // EtherCAT command data

   211

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

   212

        cur_data += command->data_size;

   213

   214

        // EtherCAT command footer

   215

        EC_WRITE_U16(cur_data, 0x0000); // Working counter

   216

        cur_data += EC_COMMAND_FOOTER_SIZE;

   217

    }

   218

   219

    if (cur_data - frame_data == EC_FRAME_HEADER_SIZE) {

   220

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

   221

        return;

   222

    }

   223

   224

    // EtherCAT frame header

   225

    EC_WRITE_U16(frame_data, ((cur_data - frame_data

   226

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

   227

   228

    // Rahmen auffüllen

   229

    while (cur_data - frame_data < EC_MIN_FRAME_SIZE)

   230

        EC_WRITE_U8(cur_data++, 0x00);

   231

   232

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

   233

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

   234

   235

    // Send frame

   236

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

   237

   238

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

   239

        end = get_cycles();

   240

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

   241

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

   242

    }

   243

}

   244

   245

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

   246

   247

/**

   248

   Wertet einen empfangenen Rahmen aus.

   249

   250

   \return 0 bei Erfolg, sonst < 0

   251

*/

   252

   253

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

   254

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

   255

                      size_t size /**< Anzahl empfangene Datenbytes */

   256

                      )

   257

{

   258

    size_t frame_size, data_size;

   259

    uint8_t command_type, command_index;

   260

    unsigned int cmd_follows, matched;

   261

    const uint8_t *cur_data;

   262

    ec_command_t *command;

   263

   264

    if (unlikely(size < EC_FRAME_HEADER_SIZE)) {

   265

        master->stats.corrupted++;

   266

        ec_master_output_stats(master);

   267

        return;

   268

    }

   269

   270

    cur_data = frame_data;

   271

   272

    // Länge des gesamten Frames prüfen

   273

    frame_size = EC_READ_U16(cur_data) & 0x07FF;

   274

    cur_data += EC_FRAME_HEADER_SIZE;

   275

   276

    if (unlikely(frame_size > size)) {

   277

        master->stats.corrupted++;

   278

        ec_master_output_stats(master);

   279

        return;

   280

    }

   281

   282

    cmd_follows = 1;

   283

    while (cmd_follows) {

   284

        // Kommando-Header auswerten

   285

        command_type  = EC_READ_U8 (cur_data);

   286

        command_index = EC_READ_U8 (cur_data + 1);

   287

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

   288

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

   289

        cur_data += EC_COMMAND_HEADER_SIZE;

   290

   291

        if (unlikely(cur_data - frame_data

   292

                     + data_size + EC_COMMAND_FOOTER_SIZE > size)) {

   293

            master->stats.corrupted++;

   294

            ec_master_output_stats(master);

   295

            return;

   296

        }

   297

   298

        // Suche passendes Kommando in der Liste

   299

        matched = 0;

   300

        list_for_each_entry(command, &master->command_queue, queue) {

   301

            if (command->state == EC_CMD_SENT

   302

                && command->type == command_type

   303

                && command->index == command_index

   304

                && command->data_size == data_size) {

   305

                matched = 1;

   306

                break;

   307

            }

   308

        }

   309

   310

        // Kein passendes Kommando in der Liste gefunden

   311

        if (!matched) {

   312

            master->stats.unmatched++;

   313

            ec_master_output_stats(master);

   314

            cur_data += data_size + EC_COMMAND_FOOTER_SIZE;

   315

            continue;

   316

        }

   317

   318

        // Empfangene Daten in Kommando-Datenspeicher kopieren

   319

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

   320

        cur_data += data_size;

   321

   322

        // Working-Counter setzen

   323

        command->working_counter = EC_READ_U16(cur_data);

   324

        cur_data += EC_COMMAND_FOOTER_SIZE;

   325

   326

        // Kommando aus der Warteschlange entfernen

   327

        command->state = EC_CMD_RECEIVED;

   328

        list_del_init(&command->queue);

   329

    }

   330

}

   331

   332

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

   333

   334

/**

   335

   Sendet ein einzelnes Kommando und wartet auf den Empfang.

   336

   337

   Wenn der Slave nicht antwortet, wird das Kommando

   338

   nochmals gesendet.

   339

   340

   \return 0 bei Erfolg, sonst < 0

   341

*/

   342

   343

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

   344

                        ec_command_t *command /**< Kommando */

   345

                        )

   346

{

   347

    unsigned int response_tries_left;

   348

   349

    response_tries_left = 10;

   350

   351

    while (1)

   352

    {

   353

        ec_master_queue_command(master, command);

   354

        ecrt_master_sync_io(master);

   355

   356

        if (command->state == EC_CMD_RECEIVED) {

   357

            if (likely(command->working_counter))

   358

                return 0;

   359

        }

   360

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

   361

            EC_ERR("Simple-IO TIMEOUT!\n");

   362

            return -1;

   363

        }

   364

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

   365

            EC_ERR("Simple-IO command error!\n");

   366

            return -1;

   367

        }

   368

   369

        // Keine direkte Antwort. Dem Slave Zeit lassen...

   370

        udelay(100);

   371

   372

        if (unlikely(--response_tries_left)) {

   373

            EC_ERR("No response in simple-IO!\n");

   374

            return -1;

   375

        }

   376

    }

   377

}

   378

   379

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

   380

   381

/**

   382

   Durchsucht den EtherCAT-Bus nach Slaves.

   383

   384

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

   385

   weiteren Betrieb notwendig sind.

   386

   387

   \return 0 bei Erfolg, sonst < 0

   388

*/

   389

   390

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

   391

{

   392

    ec_slave_t *slave;

   393

    ec_slave_ident_t *ident;

   394

    unsigned int i;

   395

    ec_command_t *command;

   396

    ec_eoe_t *eoe;

   397

   398

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

   399

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

   400

        return -1;

   401

    }

   402

   403

    command = &master->simple_command;

   404

   405

    // Determine number of slaves on bus

   406

    if (ec_command_brd(command, 0x0000, 4)) return -1;

   407

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

   408

    master->slave_count = command->working_counter;

   409

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

   410

   411

    if (!master->slave_count) return 0;

   412

   413

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

   414

          kmalloc(master->slave_count * sizeof(ec_slave_t), GFP_KERNEL))) {

   415

        EC_ERR("Failed to allocate slaves!\n");

   416

        return -1;

   417

    }

   418

   419

    // Init slaves

   420

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

   421

        slave = master->slaves + i;

   422

        ec_slave_init(slave, master);

   423

        slave->ring_position = i;

   424

        slave->station_address = i + 1;

   425

    }

   426

   427

    // For every slave on the bus

   428

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

   429

        slave = master->slaves + i;

   430

   431

        // Write station address

   432

        if (ec_command_apwr(command, slave->ring_position,

   433

                            0x0010, sizeof(uint16_t))) return -1;

   434

        EC_WRITE_U16(command->data, slave->station_address);

   435

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

   436

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

   437

            return -1;

   438

        }

   439

   440

        // Fetch all slave information

   441

        if (ec_slave_fetch(slave)) return -1;

   442

   443

        // Search for identification in "database"

   444

        ident = slave_idents;

   445

        while (ident->type) {

   446

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

   447

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

   448

                slave->type = ident->type;

   449

                break;

   450

            }

   451

            ident++;

   452

        }

   453

   454

        if (!slave->type)

   455

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

   456

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

   457

                    slave->sii_product_code, i);

   458

   459

        // Does the slave support EoE?

   460

        if (slave->sii_mailbox_protocols & EC_MBOX_EOE) {

   461

            if (!(eoe = (ec_eoe_t *) kmalloc(sizeof(ec_eoe_t), GFP_KERNEL))) {

   462

                EC_ERR("Failed to allocate EoE-Object.\n");

   463

                return -1;

   464

            }

   465

   466

            ec_eoe_init(eoe, slave);

   467

            list_add_tail(&eoe->list, &master->eoe_slaves);

   468

        }

   469

    }

   470

   471

    return 0;

   472

}

   473

   474

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

   475

   476

/**

   477

   Statistik-Ausgaben während des zyklischen Betriebs.

   478

   479

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

   480

   Betriebs bei Bedarf, aber nicht zu oft ausgegeben werden.

   481

   482

   Die Ausgabe erfolgt gesammelt höchstens einmal pro Sekunde.

   483

*/

   484

   485

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

   486

{

   487

    cycles_t t_now = get_cycles();

   488

   489

    if (unlikely((u32) (t_now - master->stats.t_last) / cpu_khz > 1000)) {

   490

        if (master->stats.timeouts) {