raw1394.c

Ieee1394驱动实现

static int arm_lock64(struct hpsb_host *host, int nodeid, octlet_t * store,
		      u64 addr, octlet_t data, octlet_t arg, int ext_tcode,
		      u16 flags)
{
	unsigned long irqflags;
	struct pending_request *req;
	struct host_info *hi;
	struct file_info *fi = NULL;
	struct list_head *entry;
	struct arm_addr *arm_addr = NULL;
	struct arm_request *arm_req = NULL;
	struct arm_response *arm_resp = NULL;
	int found = 0, size = 0, rcode = -1;
	octlet_t old, new;
	struct arm_request_response *arm_req_resp = NULL;

	if (((ext_tcode & 0xFF) == EXTCODE_FETCH_ADD) ||
	    ((ext_tcode & 0xFF) == EXTCODE_LITTLE_ADD)) {
		DBGMSG("arm_lock64 called by node: %X "
		       "addr: %4.4x %8.8x extcode: %2.2X data: %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));
	} 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_irqsave(&host_info_lock, irqflags);
	hi = find_host_info(host);	/* search addressentry in file_info's for host */
	if (hi != NULL) {
		list_for_each_entry(fi, &hi->file_info_list, 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_irqrestore(&host_info_lock, irqflags);
		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) {
		byte_t *buf1, *buf2;
		DBGMSG("arm_lock64 -> entering notification-section");
		req = __alloc_pending_request(GFP_ATOMIC);
		if (!req) {
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			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, GFP_ATOMIC);
		if (!(req->data)) {
			free_pending_request(req);
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			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)));
		buf1 = (byte_t *) arm_resp + sizeof(struct arm_response);
		buf2 = buf1 + 2 * sizeof(*store);
		if ((ext_tcode == EXTCODE_FETCH_ADD) ||
		    (ext_tcode == EXTCODE_LITTLE_ADD)) {
			arm_req->buffer_length = sizeof(*store);
			memcpy(buf1, &data, sizeof(*store));

		} else {
			arm_req->buffer_length = 2 * sizeof(*store);
			memcpy(buf1, &arg, sizeof(*store));
			memcpy(buf1 + sizeof(*store), &data, sizeof(*store));
		}
		if (rcode == RCODE_COMPLETE) {
			arm_resp->buffer_length = sizeof(*store);
			memcpy(buf2, &old, sizeof(*store));
		} else {
			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_irqrestore(&host_info_lock, irqflags);
	return (rcode);
}

static int arm_register(struct file_info *fi, struct pending_request *req)
{
	int retval;
	struct arm_addr *addr;
	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 = kmalloc(sizeof(*addr), GFP_KERNEL);
	if (!addr) {
		req->req.length = 0;
		return (-ENOMEM);
	}
	/* allocation of addr_space_buffer */
	addr->addr_space_buffer = 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_entry(fi_hlp, &hi->file_info_list, 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 */
		spin_unlock_irqrestore(&host_info_lock, flags);
		vfree(addr->addr_space_buffer);
		kfree(addr);
		return (-EALREADY);
	}
	/* another host with valid address-entry containing same addressrange */
	list_for_each_entry(hi, &host_info_list, list) {
		if (hi->host != fi->host) {
			list_for_each_entry(fi_hlp, &hi->file_info_list, 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;
				}
			}
		}
	}
	spin_unlock_irqrestore(&host_info_lock, flags);

	if (another_host) {
		DBGMSG("another hosts entry is valid -> SUCCESS");
		if (copy_to_user(int2ptr(req->req.recvb),
				 &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);
			return (-EFAULT);
		}
		free_pending_request(req);	/* immediate success or fail */
		/* INSERT ENTRY */
		spin_lock_irqsave(&host_info_lock, flags);
		list_add_tail(&addr->addr_list, &fi->addr_list);
		spin_unlock_irqrestore(&host_info_lock, flags);
		return 0;
	}
	retval =
	    hpsb_register_addrspace(&raw1394_highlevel, fi->host, &arm_ops,
				    req->req.address,
				    req->req.address + req->req.length);
	if (retval) {
		/* INSERT ENTRY */
		spin_lock_irqsave(&host_info_lock, flags);
		list_add_tail(&addr->addr_list, &fi->addr_list);
		spin_unlock_irqrestore(&host_info_lock, flags);
	} else {
		DBGMSG("arm_register failed errno: %d \n", retval);
		vfree(addr->addr_space_buffer);
		kfree(addr);
		return (-EALREADY);
	}
	free_pending_request(req);	/* immediate success or fail */
	return 0;
}

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 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_entry(hi, &host_info_list, list) {
		if (hi->host != fi->host) {
			list_for_each_entry(fi_hlp, &hi->file_info_list, 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);
		spin_unlock_irqrestore(&host_info_lock, flags);
		vfree(addr->addr_space_buffer);
		kfree(addr);
		free_pending_request(req);	/* immediate success or fail */