98 

    99     // Init kobject and add it to the hierarchy

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

   101     kobject_init(&entry->kobj);

   102     entry->kobj.ktype = &ktype_ec_sdo_entry;

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

   104     if (kobject_set_name(&entry->kobj, "%02X", entry->subindex)) {

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

   106         kobject_put(&entry->kobj);

   107         return -1;

   108     }

   109     if (kobject_add(&entry->kobj)) {

   110         EC_ERR("Failed to add entry kobject.\n");

   111         kobject_put(&entry->kobj);

   112         return -1;

   113     }

   114 

   115     return 0;

   134 

   135 /** Clear and free the Sdo entry.

   136  *

   137  * This method is called by the kobject,

   138  * once there are no more references to it.

   139  */

   140 void ec_sdo_entry_clear(

   141         struct kobject *kobj /**< Sdo entry's kobject. */

   142         )

   143 {

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

   145 

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

   147 

   148     kfree(entry);

   149 }

   150 

   151 /*****************************************************************************/

   152  

   153 /** Print Sdo entry information to a buffer.

   154  * 

   155  * \return Number of bytes written.

   156  */

   157 ssize_t ec_sdo_entry_info(

   158         ec_sdo_entry_t *entry, /**< Sdo entry. */

   159         char *buffer /**< Target buffer. */

   160         )

   161 {

   162     off_t off = 0;

   163 

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

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

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

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

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

   169 

   170     return off;

   171 }

   172 

   173 /*****************************************************************************/

   174 

   175 /** Format entry data based on the CANopen data type and print it to a buffer.

   176  *

   177  * \return number of bytes written.

   178  */

   179 ssize_t ec_sdo_entry_format_data(

   180         ec_sdo_entry_t *entry, /**< Sdo entry. */

   181         ec_sdo_request_t *request, /**< Sdo request. */

   182         char *buffer /**< Target buffer. */

   183         )

   184 {

   185     off_t off = 0;

   186     unsigned int i;

   187 

   188     if (request->data_size * 8 != entry->bit_length) {

   189         EC_ERR("Dictionary size of Sdo entry 0x%04X:%02X (%u bit) does not "

   190                 "match size of uploaded data (%u byte)!\n", entry->sdo->index,

   191                 entry->subindex, entry->bit_length, request->data_size);

   192         EC_DBG("Uploaded data:\n");

   193         ec_print_data(request->data, request->data_size);

   194         return -EIO;

   195     }

   196         

   197     if (entry->data_type == 0x0002) { // int8

   198         int8_t value;

   199         if (entry->bit_length != 8)

   200             goto not_fit;

   201         value = EC_READ_S8(request->data);

   202         off += sprintf(buffer + off, "%i (0x%02X)\n", value, value);

   203     }

   204     else if (entry->data_type == 0x0003) { // int16

   205         int16_t value;

   206         if (entry->bit_length != 16)

   207             goto not_fit;

   208         value = EC_READ_S16(request->data);

   209         off += sprintf(buffer + off, "%i (0x%04X)\n", value, value);

   210     }

   211     else if (entry->data_type == 0x0004) { // int32

   212         int32_t value;

   213         if (entry->bit_length != 32)

   214             goto not_fit;

   215         value = EC_READ_S16(request->data);

   216         off += sprintf(buffer + off, "%i (0x%08X)\n", value, value);

   217     }

   218     else if (entry->data_type == 0x0005) { // uint8

   219         uint8_t value;

   220         if (entry->bit_length != 8)

   221             goto not_fit;

   222         value = EC_READ_U8(request->data);

   223         off += sprintf(buffer + off, "%u (0x%02X)\n", value, value);

   224     }

   225     else if (entry->data_type == 0x0006) { // uint16

   226         uint16_t value;

   227         if (entry->bit_length != 16)

   228             goto not_fit;

   229         value = EC_READ_U16(request->data); 

   230         off += sprintf(buffer + off, "%u (0x%04X)\n", value, value);

   231     }

   232     else if (entry->data_type == 0x0007) { // uint32

   233         uint32_t value;

   234         if (entry->bit_length != 32)

   235             goto not_fit;

   236         value = EC_READ_U32(request->data);

   237         off += sprintf(buffer + off, "%u (0x%08X)\n", value, value);

   238     }

   239     else if (entry->data_type == 0x0009) { // string

   240         off += sprintf(buffer + off, "%s\n", request->data);

   241     }

   242     else {

   243         off += sprintf(buffer + off, "Unknown data type %04X. Data:\n",

   244                 entry->data_type);

   245         goto raw_data;

   246     }

   247     return off;

   248 

   249 not_fit:

   250     off += sprintf(buffer + off,

   251             "Invalid bit length %u for data type 0x%04X. Data:\n",

   252             entry->bit_length, entry->data_type);

   253 raw_data:

   254     for (i = 0; i < request->data_size; i++)

   255         off += sprintf(buffer + off, "%02X (%c)\n",

   256                 request->data[i], request->data[i]);

   257     return off;

   258 }

   259 

   260 /*****************************************************************************/

   261 

   262 /** Start blocking Sdo entry reading.

   263  *

   264  * This function blocks, until reading is finished, and is interruptible as

   265  * long as the master state machine has not begun with reading.

   266  *

   267  * \return number of bytes written to buffer, or error code.

   268  */

   269 ssize_t ec_sdo_entry_read_value(

   270         ec_sdo_entry_t *entry, /**< Sdo entry. */

   271         char *buffer /**< Target buffer. */

   272         )

   273 {

   274     ec_master_t *master = entry->sdo->slave->master;

   275     off_t off = 0;

   276     ec_master_sdo_request_t request;

   277 

   278     request.slave = entry->sdo->slave;

   279     ec_sdo_request_init(&request.req);

   280     ec_sdo_request_address(&request.req, entry->sdo->index, entry->subindex);

   281     ecrt_sdo_request_read(&request.req);

   282 

   283     // schedule request.

   284     down(&master->sdo_sem);

   285     list_add_tail(&request.list, &master->slave_sdo_requests);

   286     up(&master->sdo_sem);

   287 

   288     // wait for processing through FSM

   289     if (wait_event_interruptible(master->sdo_queue,

   290                 request.req.state != EC_REQUEST_QUEUED)) {

   291         // interrupted by signal

   292         down(&master->sdo_sem);

   293         if (request.req.state == EC_REQUEST_QUEUED) {

   294             list_del(&request.req.list);

   295             up(&master->sdo_sem);

   296             return -EINTR;

   297         }

   298         // request already processing: interrupt not possible.

   299         up(&master->sdo_sem);

   300     }

   301 

   302     // wait until master FSM has finished processing

   303     wait_event(master->sdo_queue, request.req.state != EC_REQUEST_BUSY);

   304 

   305     if (request.req.state != EC_REQUEST_SUCCESS)

   306         return -EIO;

   307 

   308     off += ec_sdo_entry_format_data(entry, &request.req, buffer);

   309 

   310     ec_sdo_request_clear(&request.req);

   311     return off;

   312 }

   313 

   314 /*****************************************************************************/

   315 

   316 /** Show the Sysfs attribute of an Sdo entry.

   317  *

   318  * /return Number of bytes written to buffer.

   319  */ 

   320 ssize_t ec_show_sdo_entry_attribute(

   321         struct kobject *kobj, /**< kobject. */

   322         struct attribute *attr, /**< Sysfs attribute. */

   323         char *buffer /**< Target buffer. */

   324         )

   325 {

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

   327 

   328     if (attr == &attr_info) {

   329         return ec_sdo_entry_info(entry, buffer);

   330     }

   331     else if (attr == &attr_value) {

   332         return ec_sdo_entry_read_value(entry, buffer);

   333     }

   334 

   335     return 0;

   336 }

   337 

   338 /*****************************************************************************/