master/master.c
changeset 90 044e97bce4bd
parent 89 e91ef35c36db
child 91 0120d6214948
equal deleted inserted replaced
89:e91ef35c36db 90:044e97bce4bd
   162     ec_slave_t *slave;
   162     ec_slave_t *slave;
   163     ec_slave_ident_t *ident;
   163     ec_slave_ident_t *ident;
   164     unsigned int i;
   164     unsigned int i;
   165     unsigned char data[2];
   165     unsigned char data[2];
   166 
   166 
   167     if (master->slaves || master->slave_count)
   167     if (master->slaves || master->slave_count) {
   168         EC_WARN("Slave scan already done!\n");
   168         EC_ERR("Slave scan already done!\n");
   169     ec_master_clear_slaves(master);
   169         return -1;
       
   170     }
   170 
   171 
   171     // Determine number of slaves on bus
   172     // Determine number of slaves on bus
   172 
       
   173     ec_frame_init_brd(&frame, master, 0x0000, 4);
   173     ec_frame_init_brd(&frame, master, 0x0000, 4);
   174     if (unlikely(ec_frame_send_receive(&frame) < 0)) return -1;
   174     if (unlikely(ec_frame_send_receive(&frame))) return -1;
   175 
       
   176     master->slave_count = frame.working_counter;
   175     master->slave_count = frame.working_counter;
   177     EC_INFO("Found %i slaves on bus.\n", master->slave_count);
   176     EC_INFO("Found %i slaves on bus.\n", master->slave_count);
   178 
   177 
   179     if (!master->slave_count) return 0;
   178     if (!master->slave_count) return 0;
   180 
   179 
   181     if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count
   180     if (!(master->slaves = (ec_slave_t *) kmalloc(master->slave_count
   182                                                       * sizeof(ec_slave_t),
   181                                                   * sizeof(ec_slave_t),
   183                                                       GFP_KERNEL))) {
   182                                                   GFP_KERNEL))) {
   184         EC_ERR("Could not allocate memory for bus slaves!\n");
   183         EC_ERR("Could not allocate memory for slaves!\n");
   185         return -1;
   184         return -1;
   186     }
   185     }
   187 
   186 
   188     // Init slaves
   187     // Init slaves
   189     for (i = 0; i < master->slave_count; i++) {
   188     for (i = 0; i < master->slave_count; i++) {
   202         EC_WRITE_U16(data, slave->station_address);
   201         EC_WRITE_U16(data, slave->station_address);
   203 
   202 
   204         ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010,
   203         ec_frame_init_apwr(&frame, master, slave->ring_position, 0x0010,
   205                            sizeof(uint16_t), data);
   204                            sizeof(uint16_t), data);
   206 
   205 
   207         if (unlikely(ec_frame_send_receive(&frame) < 0)) {
   206         if (unlikely(ec_frame_send_receive(&frame))) {
   208             EC_ERR("Writing station address failed on slave %i!\n", i);
   207             EC_ERR("Writing station address failed on slave %i!\n", i);
   209             return -1;
   208             return -1;
   210         }
   209         }
   211 
   210 
   212         // Fetch all slave information
   211         // Fetch all slave information
   484         // Resetting FMMU's
   483         // Resetting FMMU's
   485         if (slave->base_fmmu_count) {
   484         if (slave->base_fmmu_count) {
   486             memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
   485             memset(data, 0x00, EC_FMMU_SIZE * slave->base_fmmu_count);
   487             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600,
   486             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0600,
   488                                EC_FMMU_SIZE * slave->base_fmmu_count, data);
   487                                EC_FMMU_SIZE * slave->base_fmmu_count, data);
   489             if (unlikely(ec_frame_send_receive(&frame) < 0)) {
   488             if (unlikely(ec_frame_send_receive(&frame))) {
   490                 EC_ERR("Resetting FMMUs failed on slave %i!\n",
   489                 EC_ERR("Resetting FMMUs failed on slave %i!\n",
   491                        slave->ring_position);
   490                        slave->ring_position);
   492                 return -1;
   491                 return -1;
   493             }
   492             }
   494         }
   493         }
   496         // Resetting Sync Manager channels
   495         // Resetting Sync Manager channels
   497         if (slave->base_sync_count) {
   496         if (slave->base_sync_count) {
   498             memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
   497             memset(data, 0x00, EC_SYNC_SIZE * slave->base_sync_count);
   499             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800,
   498             ec_frame_init_npwr(&frame, master, slave->station_address, 0x0800,
   500                                EC_SYNC_SIZE * slave->base_sync_count, data);
   499                                EC_SYNC_SIZE * slave->base_sync_count, data);
   501             if (unlikely(ec_frame_send_receive(&frame) < 0)) {
   500             if (unlikely(ec_frame_send_receive(&frame))) {
   502                 EC_ERR("Resetting sync managers failed on slave %i!\n",
   501                 EC_ERR("Resetting sync managers failed on slave %i!\n",
   503                        slave->ring_position);
   502                        slave->ring_position);
   504                 return -1;
   503                 return -1;
   505             }
   504             }
   506         }
   505         }