- if (result < 0) {
- printk("%s: cannot read oPCR\n", __func__);
- return result;
- } else {
-/* printk(KERN_INFO "%s: oPCR = %08x\n",__func__,test_oPCR); */
- do {
- OPCR *hilf= (OPCR*) &test_oPCR;
-
- if (!hilf->OnLine) {
- printk("%s: Output offline; oPCR: %08x\n", __func__, test_oPCR);
- return -EBUSY;
- } else {
- quadlet_t new_oPCR;
-
- old_oPCR=test_oPCR;
- if (hilf->PTPConnCount) {
- if (hilf->ChNr != iso_channel) {
- printk("%s: Output plug has already connection on channel %u; cannot change it to channel %u\n",__func__,hilf->ChNr,iso_channel);
- return -EBUSY;
- } else
- printk(KERN_INFO "%s: Overlaying existing connection; connection counter was: %u\n",__func__, hilf->PTPConnCount);
- BWU=0; //we allocate no bandwidth (is this necessary?)
- } else {
- hilf->ChNr=iso_channel;
- hilf->DataRate=FIRESAT_SPEED;
-
- hilf->OvhdID=0; //FIXME: that is for worst case -> optimize
- BWU=hilf->OvhdID?hilf->OvhdID*32:512;
- BWU += (hilf->PayloadLo + (hilf->PayloadHi << 8) +3) * (2 << (3-hilf->DataRate));
-/* if (allocate_1394_resources(iso_channel,BWU))
- {
- cout << "Allocation of resources failed\n";
- return -2;
- }*/
- }
-
- hilf->PTPConnCount++;
- new_oPCR=test_oPCR;
-/* printk(KERN_INFO "%s: trying compare_swap...\n",__func__); */
-/* printk(KERN_INFO "%s: oPCR_old: %08x, oPCR_new: %08x\n",__func__, old_oPCR, new_oPCR); */
- result=cmp_lock(firesat, &test_oPCR, oPCR_address, old_oPCR, 2);
-
- if (result < 0) {
- printk("%s: cannot compare_swap oPCR\n",__func__);
- return result;
- }
- if ((old_oPCR != test_oPCR) && (!((OPCR*) &old_oPCR)->PTPConnCount))
- {
- printk("%s: change of oPCR failed -> freeing resources\n",__func__);
-// hilf= (OPCR*) &new_oPCR;
-// unsigned int BWU=hilf->OvhdID?hilf->OvhdID*32:512;
-// BWU += (hilf->Payload+3) * (2 << (3-hilf->DataRate));
-/* if (deallocate_1394_resources(iso_channel,BWU))
- {
-
- cout << "Deallocation of resources failed\n";
- return -3;
- }*/
- }
- }
+int cmp_establish_pp_connection(struct firesat *firesat, int plug, int channel)
+{
+ __be32 old_opcr, opcr;
+ u64 opcr_address = CMP_OUTPUT_PLUG_CONTROL_REG_0 + (plug << 2);
+ int attempts = 0;
+ int ret;
+
+ ret = cmp_read(firesat, &opcr, opcr_address, 4);
+ if (ret < 0)
+ return ret;
+
+repeat:
+ if (!get_opcr_online(opcr)) {
+ dev_err(&firesat->ud->device, "CMP: output offline\n");
+ return -EBUSY;
+ }
+
+ old_opcr = opcr;
+
+ if (get_opcr_p2p_connections(opcr)) {
+ if (get_opcr_channel(opcr) != channel) {
+ dev_err(&firesat->ud->device,
+ "CMP: cannot change channel\n");
+ return -EBUSY;