raw1394.c

1394在linux下单独的驱动程序代码

			nodeid, (u16) ((addr >>32) & 0xFFFF),
			(u32) (addr & 0xFFFFFFFF), 
			ext_tcode & 0xFF , 
			(u32) ((be64_to_cpu(data) >> 32) & 0xFFFFFFFF), 
			(u32) (be64_to_cpu(data) & 0xFFFFFFFF));
	} else {
		DBGMSG("arm_lock64 called by node: %X "
			"addr: %4.4x %8.8x extcode: %2.2X data: %8.8X %8.8X arg: "
			"%8.8X %8.8X ",
			nodeid, (u16) ((addr >>32) & 0xFFFF),
			(u32) (addr & 0xFFFFFFFF), 
			ext_tcode & 0xFF , 
			(u32) ((be64_to_cpu(data) >> 32) & 0xFFFFFFFF), 
			(u32) (be64_to_cpu(data) & 0xFFFFFFFF),
			(u32) ((be64_to_cpu(arg)  >> 32) & 0xFFFFFFFF), 
			(u32) (be64_to_cpu(arg)  & 0xFFFFFFFF));
	}
	spin_lock(&host_info_lock);
	hi = find_host_info(host); /* search addressentry in file_info's for host */
	if (hi != NULL) {
		list_for_each(lh, &hi->file_info_list) {
			fi = list_entry(lh, struct file_info, list);
			entry = fi->addr_list.next;
			while (entry != &(fi->addr_list)) {
				arm_addr = list_entry(entry, struct arm_addr, addr_list);
				if (((arm_addr->start) <= (addr)) && 
					((arm_addr->end) >= (addr+sizeof(*store)))) {
					found = 1;
					break;
				}
				entry = entry->next;
			}
			if (found) {
				break;
			}
		}
	}
	rcode = -1;
	if (!found) {
		printk(KERN_ERR "raw1394: arm_lock64 FAILED addr_entry not found"
			" -> rcode_address_error\n");
		spin_unlock(&host_info_lock);
		return (RCODE_ADDRESS_ERROR);
	} else {
		DBGMSG("arm_lock64 addr_entry FOUND");
	}
	if (rcode == -1) {
		if (arm_addr->access_rights & ARM_LOCK) {
			if (!(arm_addr->client_transactions & ARM_LOCK)) {
				memcpy(&old,(arm_addr->addr_space_buffer)+(addr-(arm_addr->start)),
					sizeof(old));
				switch (ext_tcode) {
				case (EXTCODE_MASK_SWAP):
					new = data | (old & ~arg);
					break;
				case (EXTCODE_COMPARE_SWAP):
					if (old == arg) {
						new = data;
					} else {
						new = old;
					}
					break;
				case (EXTCODE_FETCH_ADD):
					new = cpu_to_be64(be64_to_cpu(data) + be64_to_cpu(old));
					break;
				case (EXTCODE_LITTLE_ADD):
					new = cpu_to_le64(le64_to_cpu(data) + le64_to_cpu(old));
					break;
				case (EXTCODE_BOUNDED_ADD):
					if (old != arg) {
						new = cpu_to_be64(be64_to_cpu(data) + 
							be64_to_cpu(old));
					} else {
						new = old;
					}
					break;
				case (EXTCODE_WRAP_ADD):
					if (old != arg) {
						new = cpu_to_be64(be64_to_cpu(data) + 
							be64_to_cpu(old));
					} else {
						new = data;
					}
					break;
				default:
					printk(KERN_ERR "raw1394: arm_lock64 FAILED "
						"ext_tcode not allowed -> rcode_type_error\n");
					rcode = RCODE_TYPE_ERROR; /* function not allowed */
					break;
				} /*switch*/
				if (rcode == -1) {
					DBGMSG("arm_lock64 -> (rcode_complete)");
					rcode = RCODE_COMPLETE;
					memcpy (store, &old, sizeof(*store));
					memcpy ((arm_addr->addr_space_buffer)+
						(addr-(arm_addr->start)), 
						&new, sizeof(*store));
				} 
			}
		} else {
			rcode = RCODE_TYPE_ERROR; /* function not allowed */
			DBGMSG("arm_lock64 -> rcode_type_error (access denied)");
		}
	}
	if (arm_addr->notification_options & ARM_LOCK) {
		DBGMSG("arm_lock64 -> entering notification-section");
		req = __alloc_pending_request(SLAB_ATOMIC);
		if (!req) {
			spin_unlock(&host_info_lock);
			DBGMSG("arm_lock64 -> rcode_conflict_error");
			return(RCODE_CONFLICT_ERROR); /* A resource conflict was detected. 
			The request may be retried */
		}
		size =  sizeof(struct arm_request)+sizeof(struct arm_response) +
			3 * sizeof(*store) +
			sizeof (struct arm_request_response); /* maximum */
		req->data = kmalloc(size, SLAB_ATOMIC);
		if (!(req->data)) {
			free_pending_request(req);
			spin_unlock(&host_info_lock);
			DBGMSG("arm_lock64 -> rcode_conflict_error");
			return(RCODE_CONFLICT_ERROR); /* A resource conflict was detected. 
			The request may be retried */
		}
		req->free_data=1;
		arm_req_resp = (struct arm_request_response *) (req->data);
		arm_req  = (struct arm_request *) ((byte_t *)(req->data) + 
			(sizeof (struct arm_request_response)));
		arm_resp = (struct arm_response *) ((byte_t *)(arm_req) + 
			(sizeof(struct arm_request)));
		arm_req->buffer = ((byte_t *)(arm_resp) + 
			(sizeof(struct arm_response)));
		arm_resp->buffer = ((byte_t *)(arm_req->buffer) + 
			(2* sizeof(*store)));
		if ((ext_tcode == EXTCODE_FETCH_ADD) || 
			(ext_tcode == EXTCODE_LITTLE_ADD)) {
			arm_req->buffer_length = sizeof(*store);
			memcpy (arm_req->buffer, &data, sizeof(*store));
			
		} else {
			arm_req->buffer_length = 2 * sizeof(*store);
			memcpy (arm_req->buffer, &arg,  sizeof(*store));
			memcpy (((arm_req->buffer) + sizeof(*store)), 
				&data, sizeof(*store));
		}
		if (rcode == RCODE_COMPLETE) {
			arm_resp->buffer_length = sizeof(*store);
			memcpy (arm_resp->buffer, &old, sizeof(*store));
		} else {
			arm_resp->buffer = NULL;
			arm_resp->buffer_length = 0;
		}
		req->file_info = fi;
		req->req.type = RAW1394_REQ_ARM;
		req->req.generation = get_hpsb_generation(host);
		req->req.misc = ( (((sizeof(*store)) << 16) & (0xFFFF0000)) | 
			(ARM_LOCK & 0xFF));
		req->req.tag  = arm_addr->arm_tag;
		req->req.recvb = arm_addr->recvb;
		req->req.length = size;
		arm_req->generation = req->req.generation;
		arm_req->extended_transaction_code = ext_tcode;
		arm_req->destination_offset = addr;
		arm_req->source_nodeid = nodeid;
		arm_req->destination_nodeid = host->node_id;
		arm_req->tlabel = (flags >> 10) & 0x3f;
		arm_req->tcode = (flags >> 4) & 0x0f;
		arm_resp->response_code = rcode;
		arm_req_resp->request  = int2ptr((arm_addr->recvb) + 
			sizeof (struct arm_request_response));
		arm_req_resp->response = int2ptr((arm_addr->recvb) + 
			sizeof (struct arm_request_response) +
			sizeof (struct arm_request));
		arm_req->buffer = int2ptr((arm_addr->recvb) + 
			sizeof (struct arm_request_response) +
			sizeof (struct arm_request) +
			sizeof (struct arm_response));
		arm_resp->buffer = int2ptr((arm_addr->recvb) + 
			sizeof (struct arm_request_response) +
			sizeof (struct arm_request) +
			sizeof (struct arm_response) +
			2* sizeof (*store));
		queue_complete_req(req);
	}
	spin_unlock(&host_info_lock);
	return(rcode);
}

static int arm_register(struct file_info *fi, struct pending_request *req)
{
	int retval;
	struct arm_addr *addr;
	struct list_head *lh, *lh_1, *lh_2;
	struct host_info *hi;
	struct file_info *fi_hlp = NULL;
	struct list_head *entry;
	struct arm_addr  *arm_addr = NULL;
	int same_host, another_host;
	unsigned long flags;
	
	DBGMSG("arm_register called "
		"addr(Offset): %8.8x %8.8x length: %u "
		"rights: %2.2X notify: %2.2X "
		"max_blk_len: %4.4X",
		(u32) ((req->req.address >>32) & 0xFFFF),
		(u32) (req->req.address & 0xFFFFFFFF),
		req->req.length, ((req->req.misc >> 8) & 0xFF),
		(req->req.misc & 0xFF),((req->req.misc >> 16) & 0xFFFF));
	/* check addressrange */
	if ((((req->req.address) & ~(0xFFFFFFFFFFFFULL)) != 0) ||
		(((req->req.address + req->req.length) & ~(0xFFFFFFFFFFFFULL)) != 0)) {
		req->req.length = 0;
		return (-EINVAL);
	}
	/* addr-list-entry for fileinfo */
	addr = (struct arm_addr *)kmalloc(sizeof(struct arm_addr), SLAB_KERNEL); 
	if (!addr) {
		req->req.length = 0;
		return (-ENOMEM);
	} 
	/* allocation of addr_space_buffer */
	addr->addr_space_buffer = (u8 *)vmalloc(req->req.length);
	if (!(addr->addr_space_buffer)) {
		kfree(addr);
		req->req.length = 0;
		return (-ENOMEM);
	}
	/* initialization of addr_space_buffer */
	if ((req->req.sendb)== (unsigned long)NULL) {
		/* init: set 0 */
		memset(addr->addr_space_buffer, 0,req->req.length);
	} else {
		/* init: user -> kernel */
		if (copy_from_user(addr->addr_space_buffer,int2ptr(req->req.sendb),
			req->req.length)) {
			vfree(addr->addr_space_buffer);
			kfree(addr);
			return (-EFAULT);
		}
	}
	INIT_LIST_HEAD(&addr->addr_list);
	addr->arm_tag   = req->req.tag;
	addr->start     = req->req.address;
	addr->end       = req->req.address + req->req.length;
	addr->access_rights = (u8) (req->req.misc & 0x0F);
	addr->notification_options = (u8) ((req->req.misc >> 4) & 0x0F);
	addr->client_transactions = (u8) ((req->req.misc >> 8) & 0x0F);
	addr->access_rights |= addr->client_transactions;
	addr->notification_options |= addr->client_transactions;
	addr->recvb     = req->req.recvb;
	addr->rec_length = (u16) ((req->req.misc >> 16) & 0xFFFF);
	spin_lock_irqsave(&host_info_lock, flags);
	hi = find_host_info(fi->host);
	same_host = 0;
	another_host = 0;
	/* same host with address-entry containing same addressrange ? */
	list_for_each(lh, &hi->file_info_list) {
		fi_hlp = list_entry(lh, struct file_info, list);
		entry = fi_hlp->addr_list.next;
		while (entry != &(fi_hlp->addr_list)) {
			arm_addr = list_entry(entry, struct arm_addr, addr_list);
			if ( (arm_addr->start == addr->start) && 
				(arm_addr->end == addr->end)) {
				DBGMSG("same host ownes same "
					"addressrange -> EALREADY");
				same_host = 1;
				break;
			}
			entry = entry->next;
		}
		if (same_host) {
			break;
		}
	}
	if (same_host) {
		/* addressrange occupied by same host */
		vfree(addr->addr_space_buffer);
		kfree(addr);
		spin_unlock_irqrestore(&host_info_lock, flags);
		return (-EALREADY);
	}
	/* another host with valid address-entry containing same addressrange */
	list_for_each(lh_1, &host_info_list) {
		hi = list_entry(lh_1, struct host_info, list);
		if (hi->host != fi->host) {
			list_for_each(lh_2, &hi->file_info_list) {
				fi_hlp = list_entry(lh_2, struct file_info, list);
				entry = fi_hlp->addr_list.next;
				while (entry != &(fi_hlp->addr_list)) {
					arm_addr = list_entry(entry, struct arm_addr, addr_list);
					if ( (arm_addr->start == addr->start) && 
						(arm_addr->end == addr->end)) {
						DBGMSG("another host ownes same "
							"addressrange");
						another_host = 1;
						break;
					}
					entry = entry->next;
				}
				if (another_host) {
					break;
				}
			}
		}
	}
	if (another_host) {
		DBGMSG("another hosts entry is valid -> SUCCESS");
		if (copy_to_user(int2ptr(req->req.recvb),
			int2ptr(&addr->start),sizeof(u64))) {
			printk(KERN_ERR "raw1394: arm_register failed "
				" address-range-entry is invalid -> EFAULT !!!\n");
			vfree(addr->addr_space_buffer);
			kfree(addr);
			spin_unlock_irqrestore(&host_info_lock, flags);
			return (-EFAULT);
		}
		free_pending_request(req); /* immediate success or fail */
		/* INSERT ENTRY */
		list_add_tail(&addr->addr_list, &fi->addr_list);
		spin_unlock_irqrestore(&host_info_lock, flags);
		return sizeof(struct raw1394_request);
	}
	retval = hpsb_register_addrspace(&raw1394_highlevel, &arm_ops, req->req.address,
		req->req.address + req->req.length);
	if (retval) {
		/* INSERT ENTRY */
		list_add_tail(&addr->addr_list, &fi->addr_list);
	} else {
		DBGMSG("arm_register failed errno: %d \n",retval);
		vfree(addr->addr_space_buffer);
		kfree(addr);
		spin_unlock_irqrestore(&host_info_lock, flags);
		return (-EALREADY); 
	}
	spin_unlock_irqrestore(&host_info_lock, flags);
	free_pending_request(req); /* immediate success or fail */
	return sizeof(struct raw1394_request);
}

static int arm_unregister(struct file_info *fi, struct pending_request *req)
{
	int found  = 0;
	int retval = 0;
	struct list_head *entry;
	struct arm_addr  *addr = NULL;
	struct list_head *lh_1, *lh_2;
	struct host_info *hi;
	struct file_info *fi_hlp = NULL;
	struct arm_addr  *arm_addr = NULL;
	int another_host;
	unsigned long flags;
	
	DBGMSG("arm_Unregister called addr(Offset): "
		"%8.8x %8.8x",
		(u32) ((req->req.address >>32) & 0xFFFF),
		(u32) (req->req.address & 0xFFFFFFFF));
	spin_lock_irqsave(&host_info_lock, flags);
	/* get addr */
	entry = fi->addr_list.next;
	while (entry != &(fi->addr_list)) {
		addr = list_entry(entry, struct arm_addr, addr_list);
		if (addr->start == req->req.address) {
			found = 1;
			break;
		}
		entry = entry->next;
	}
	if (!found) {
		DBGMSG("arm_Unregister addr not found");
		spin_unlock_irqrestore(&host_info_lock, flags);
		return (-EINVAL);
	}
	DBGMSG("arm_Unregister addr found");
	another_host = 0;
	/* another host with valid address-entry containing 
	same addressrange */
	list_for_each(lh_1, &host_info_list) {
		hi = list_entry(lh_1, struct host_info, list);
		if (hi->host != fi->host) {
			list_for_each(lh_2, &hi->file_info_list) {
				fi_hlp = list_entry(lh_2, struct file_info, list);
				entry = fi_hlp->addr_list.next;
				while (entry != &(fi_hlp->addr_list)) {
					arm_addr = list_entry(entry, 
						struct arm_addr, addr_list);
					if (arm_addr->start == 
						addr->start) {
						DBGMSG("another host ownes "
							"same addressrange");
						another_host = 1;
						break;
					}
					entry = entry->next;
				}
				if (another_host) {
					break;
				}
			}
		}
	}
	if (another_host) {
		DBGMSG("delete entry from list -> success");
		list_del(&addr->addr_list);
		vfree(addr->addr_space_buffer);
		kfree(addr);
		free_pending_request(req); /* immediate success or fail */
		spin_unlock_irqrestore(&host_info_lock, flags);
		return sizeof(struct raw1394_request);
	} 
	retval = hpsb_unregister_addrspace(&raw1394_highlevel, addr->start);
	if (!retval) {
		printk(KERN_ERR "raw1394: arm_Unregister failed -> EINVAL\n");
		spin_unlock_irqrestore(&host_info_lock, flags);
		return (-EINVAL);
	}
	DBGMSG("delete entry from list -> success");
	list_del(&addr->addr_list);
	spin_unlock_irqrestore(&host_info_lock, flags);
	vfree(addr->addr_space_buffer);
	kfree(addr);
	free_pending_request(req); /* immediate success or fail */
	return sizeof(struct raw1394_request);
}

static int reset_notification(struct file_info *fi, struct pending_request *req)
{
	DBGMSG("reset_notification called - switch %s ",
		(req->req.misc == RAW1394_NOTIFY_OFF)?"OFF":"ON");
	if ((req->req.misc == RAW1394_NOTIFY_OFF) ||
		(req->req.misc == RAW1394_NOTIFY_ON)) {
		fi->notification=(u8)req->req.misc;
		free_pending_request(req); /* we have to free the request, because we queue no response, and therefore nobody will free it */
		return sizeof(struct raw1394_request);
	} 
	/* error EINVAL (22) invalid argument */
	return (-EINVAL);
}

static int write_phypacket(struct file_info *fi, struct pending_request *req)
{
	struct hpsb_packet *packet = NULL;
	int retval=0;
	quadlet_t data;
	
	data = be32_to_cpu((u32)req->req.sendb);
	DBGMSG("write_phypacket called - quadlet 0x%8.8x ",data);
	packet = hpsb_make_phypacket (fi->host, data);
	if (!packet) return -ENOMEM;
	req->req.length=0;
	req->packet=packet;
	hpsb_set_packet_complete_task(packet, (void(*)(void*))queue_complete_cb, req);
	spin_lock_irq(&fi->reqlists_lock);
	list_add_tail(&req->list, &fi->req_pending);
	spin_unlock_irq(&fi->reqlists_lock);
	packet->generation = req->req.generation;
	retval = hpsb_send_packet(packet);
	DBGMSG("write_phypacket send_packet called => retval: %d ",
		retval);
	if (! retval) {