783 ssize_t ec_slave_write_eeprom(ec_slave_t *slave, /**< EtherCAT slave */ |
780 ssize_t ec_slave_write_eeprom(ec_slave_t *slave, /**< EtherCAT slave */ |
784 const uint8_t *data, /**< new EEPROM data */ |
781 const uint8_t *data, /**< new EEPROM data */ |
785 size_t size /**< size of data in bytes */ |
782 size_t size /**< size of data in bytes */ |
786 ) |
783 ) |
787 { |
784 { |
788 uint16_t word_size, cat_type, cat_size; |
785 ec_eeprom_write_request_t request; |
789 const uint16_t *data_words, *next_header; |
786 const uint16_t *cat_header; |
790 uint16_t *new_data; |
787 uint16_t cat_type, cat_size; |
791 |
788 ec_master_t *master = slave->master; |
792 if (slave->master->mode != EC_MASTER_MODE_IDLE) { |
789 |
|
790 if (slave->master->mode != EC_MASTER_MODE_IDLE) { // FIXME |
793 EC_ERR("Writing EEPROMs only allowed in idle mode!\n"); |
791 EC_ERR("Writing EEPROMs only allowed in idle mode!\n"); |
794 return -EACCES; |
|
795 } |
|
796 |
|
797 if (slave->new_eeprom_data) { |
|
798 EC_ERR("Slave %i already has a pending EEPROM write operation!\n", |
|
799 slave->ring_position); |
|
800 return -EBUSY; |
792 return -EBUSY; |
801 } |
793 } |
802 |
794 |
803 // coarse check of the data |
|
804 |
|
805 if (size % 2) { |
795 if (size % 2) { |
806 EC_ERR("EEPROM size is odd! Dropping.\n"); |
796 EC_ERR("EEPROM data size is odd! Dropping.\n"); |
807 return -EINVAL; |
797 return -EINVAL; |
808 } |
798 } |
809 |
799 |
810 data_words = (const uint16_t *) data; |
800 // init EEPROM write request |
811 word_size = size / 2; |
801 INIT_LIST_HEAD(&request.list); |
812 |
802 request.slave = slave; |
813 if (word_size < 0x0041) { |
803 request.words = (const uint16_t *) data; |
|
804 request.offset = 0; |
|
805 request.size = size / 2; |
|
806 request.state = EC_EEPROM_REQ_QUEUED; |
|
807 |
|
808 if (request.size < 0x0041) { |
814 EC_ERR("EEPROM data too short! Dropping.\n"); |
809 EC_ERR("EEPROM data too short! Dropping.\n"); |
815 return -EINVAL; |
810 return -EINVAL; |
816 } |
811 } |
817 |
812 |
818 next_header = data_words + 0x0040; |
813 cat_header = request.words + 0x0040; // first category header |
819 cat_type = EC_READ_U16(next_header); |
814 cat_type = EC_READ_U16(cat_header); |
820 while (cat_type != 0xFFFF) { |
815 while (cat_type != 0xFFFF) { // cycle through categories |
821 cat_type = EC_READ_U16(next_header); |
816 if (cat_header + 1 > request.words + request.size) { |
822 cat_size = EC_READ_U16(next_header + 1); |
817 EC_ERR("EEPROM data corrupted! Dropping.\n"); |
823 if ((next_header + cat_size + 2) - data_words >= word_size) { |
|
824 EC_ERR("EEPROM data seems to be corrupted! Dropping.\n"); |
|
825 return -EINVAL; |
818 return -EINVAL; |
826 } |
819 } |
827 next_header += cat_size + 2; |
820 cat_size = EC_READ_U16(cat_header + 1); |
828 cat_type = EC_READ_U16(next_header); |
821 if (cat_header + cat_size + 2 > request.words + request.size) { |
829 } |
822 EC_ERR("EEPROM data corrupted! Dropping.\n"); |
830 |
823 return -EINVAL; |
831 // data ok! |
824 } |
832 |
825 cat_header += cat_size + 2; |
833 if (!(new_data = (uint16_t *) kmalloc(word_size * 2, GFP_KERNEL))) { |
826 cat_type = EC_READ_U16(cat_header); |
834 EC_ERR("Unable to allocate memory for new EEPROM data!\n"); |
827 } |
835 return -ENOMEM; |
828 |
836 } |
829 // data ok: schedule EEPROM write request. |
837 memcpy(new_data, data, size); |
830 down(&master->eeprom_sem); |
838 |
831 list_add_tail(&request.list, &master->eeprom_requests); |
839 slave->new_eeprom_size = word_size; |
832 up(&master->eeprom_sem); |
840 slave->new_eeprom_data = new_data; |
833 |
841 |
834 // wait for processing through FSM |
842 EC_INFO("EEPROM writing scheduled for slave %i, %i words.\n", |
835 while (wait_event_interruptible(master->eeprom_queue, |
843 slave->ring_position, word_size); |
836 request.state != EC_EEPROM_REQ_QUEUED)) { |
844 return size; |
837 // interrupted by signal |
|
838 down(&master->eeprom_sem); |
|
839 if (!list_empty(&request.list)) { |
|
840 // still queued: safely dequeue |
|
841 list_del(&request.list); |
|
842 up(&master->eeprom_sem); |
|
843 return -EINTR; |
|
844 } |
|
845 // request processing: interrupt not possible. |
|
846 up(&master->eeprom_sem); |
|
847 } |
|
848 |
|
849 return request.state == EC_EEPROM_REQ_COMPLETED ? size : -EIO; |
845 } |
850 } |
846 |
851 |
847 /*****************************************************************************/ |
852 /*****************************************************************************/ |
848 |
853 |
849 /** |
854 /** |