raw1394.c

Ieee1394驱动实现

				       (arm_addr->addr_space_buffer) + (addr -
									(arm_addr->
									 start)),
				       length);
				DBGMSG("arm_read -> (rcode_complete)");
				rcode = RCODE_COMPLETE;
			}
		} else {
			rcode = RCODE_TYPE_ERROR;	/* function not allowed */
			DBGMSG("arm_read -> rcode_type_error (access denied)");
		}
	}
	if (arm_addr->notification_options & ARM_READ) {
		DBGMSG("arm_read -> entering notification-section");
		req = __alloc_pending_request(GFP_ATOMIC);
		if (!req) {
			DBGMSG("arm_read -> rcode_conflict_error");
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			return (RCODE_CONFLICT_ERROR);	/* A resource conflict was detected.
							   The request may be retried */
		}
		if (rcode == RCODE_COMPLETE) {
			size =
			    sizeof(struct arm_request) +
			    sizeof(struct arm_response) +
			    length * sizeof(byte_t) +
			    sizeof(struct arm_request_response);
		} else {
			size =
			    sizeof(struct arm_request) +
			    sizeof(struct arm_response) +
			    sizeof(struct arm_request_response);
		}
		req->data = kmalloc(size, GFP_ATOMIC);
		if (!(req->data)) {
			free_pending_request(req);
			DBGMSG("arm_read -> rcode_conflict_error");
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			return (RCODE_CONFLICT_ERROR);	/* A resource conflict was detected.
							   The request may be retried */
		}
		req->free_data = 1;
		req->file_info = fi;
		req->req.type = RAW1394_REQ_ARM;
		req->req.generation = get_hpsb_generation(host);
		req->req.misc =
		    (((length << 16) & (0xFFFF0000)) | (ARM_READ & 0xFF));
		req->req.tag = arm_addr->arm_tag;
		req->req.recvb = arm_addr->recvb;
		req->req.length = size;
		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 = NULL;
		arm_resp->buffer = NULL;
		if (rcode == RCODE_COMPLETE) {
			byte_t *buf =
			    (byte_t *) arm_resp + sizeof(struct arm_response);
			memcpy(buf,
			       (arm_addr->addr_space_buffer) + (addr -
								(arm_addr->
								 start)),
			       length);
			arm_resp->buffer =
			    int2ptr((arm_addr->recvb) +
				    sizeof(struct arm_request_response) +
				    sizeof(struct arm_request) +
				    sizeof(struct arm_response));
		}
		arm_resp->buffer_length =
		    (rcode == RCODE_COMPLETE) ? length : 0;
		arm_resp->response_code = rcode;
		arm_req->buffer_length = 0;
		arm_req->generation = req->req.generation;
		arm_req->extended_transaction_code = 0;
		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_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));
		queue_complete_req(req);
	}
	spin_unlock_irqrestore(&host_info_lock, irqflags);
	return (rcode);
}

static int arm_write(struct hpsb_host *host, int nodeid, int destid,
		     quadlet_t * data, u64 addr, size_t length, 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, length_conflict = 0;
	struct arm_request_response *arm_req_resp = NULL;

	DBGMSG("arm_write called by node: %X"
	       "addr: %4.4x %8.8x length: %Zu", nodeid,
	       (u16) ((addr >> 32) & 0xFFFF), (u32) (addr & 0xFFFFFFFF),
	       length);
	spin_lock_irqsave(&host_info_lock, irqflags);
	hi = find_host_info(host);	/* search address-entry */
	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 + length))) {
					found = 1;
					break;
				}
				entry = entry->next;
			}
			if (found) {
				break;
			}
		}
	}
	rcode = -1;
	if (!found) {
		printk(KERN_ERR "raw1394: arm_write FAILED addr_entry not found"
		       " -> rcode_address_error\n");
		spin_unlock_irqrestore(&host_info_lock, irqflags);
		return (RCODE_ADDRESS_ERROR);
	} else {
		DBGMSG("arm_write addr_entry FOUND");
	}
	if (arm_addr->rec_length < length) {
		DBGMSG("arm_write blocklength too big -> rcode_data_error");
		length_conflict = 1;
		rcode = RCODE_DATA_ERROR;	/* hardware error, data is unavailable */
	}
	if (rcode == -1) {
		if (arm_addr->access_rights & ARM_WRITE) {
			if (!(arm_addr->client_transactions & ARM_WRITE)) {
				memcpy((arm_addr->addr_space_buffer) +
				       (addr - (arm_addr->start)), data,
				       length);
				DBGMSG("arm_write -> (rcode_complete)");
				rcode = RCODE_COMPLETE;
			}
		} else {
			rcode = RCODE_TYPE_ERROR;	/* function not allowed */
			DBGMSG("arm_write -> rcode_type_error (access denied)");
		}
	}
	if (arm_addr->notification_options & ARM_WRITE) {
		DBGMSG("arm_write -> entering notification-section");
		req = __alloc_pending_request(GFP_ATOMIC);
		if (!req) {
			DBGMSG("arm_write -> rcode_conflict_error");
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			return (RCODE_CONFLICT_ERROR);	/* A resource conflict was detected.
							   The request my be retried */
		}
		size =
		    sizeof(struct arm_request) + sizeof(struct arm_response) +
		    (length) * sizeof(byte_t) +
		    sizeof(struct arm_request_response);
		req->data = kmalloc(size, GFP_ATOMIC);
		if (!(req->data)) {
			free_pending_request(req);
			DBGMSG("arm_write -> rcode_conflict_error");
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			return (RCODE_CONFLICT_ERROR);	/* A resource conflict was detected.
							   The request may be retried */
		}
		req->free_data = 1;
		req->file_info = fi;
		req->req.type = RAW1394_REQ_ARM;
		req->req.generation = get_hpsb_generation(host);
		req->req.misc =
		    (((length << 16) & (0xFFFF0000)) | (ARM_WRITE & 0xFF));
		req->req.tag = arm_addr->arm_tag;
		req->req.recvb = arm_addr->recvb;
		req->req.length = size;
		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_resp->buffer = NULL;
		memcpy((byte_t *) arm_resp + sizeof(struct arm_response),
		       data, length);
		arm_req->buffer = int2ptr((arm_addr->recvb) +
					  sizeof(struct arm_request_response) +
					  sizeof(struct arm_request) +
					  sizeof(struct arm_response));
		arm_req->buffer_length = length;
		arm_req->generation = req->req.generation;
		arm_req->extended_transaction_code = 0;
		arm_req->destination_offset = addr;
		arm_req->source_nodeid = nodeid;
		arm_req->destination_nodeid = destid;
		arm_req->tlabel = (flags >> 10) & 0x3f;
		arm_req->tcode = (flags >> 4) & 0x0f;
		arm_resp->buffer_length = 0;
		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));
		queue_complete_req(req);
	}
	spin_unlock_irqrestore(&host_info_lock, irqflags);
	return (rcode);
}

static int arm_lock(struct hpsb_host *host, int nodeid, quadlet_t * store,
		    u64 addr, quadlet_t data, quadlet_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;
	quadlet_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_lock  called by node: %X "
		       "addr: %4.4x %8.8x extcode: %2.2X data: %8.8X",
		       nodeid, (u16) ((addr >> 32) & 0xFFFF),
		       (u32) (addr & 0xFFFFFFFF), ext_tcode & 0xFF,
		       be32_to_cpu(data));
	} else {
		DBGMSG("arm_lock  called by node: %X "
		       "addr: %4.4x %8.8x extcode: %2.2X data: %8.8X arg: %8.8X",
		       nodeid, (u16) ((addr >> 32) & 0xFFFF),
		       (u32) (addr & 0xFFFFFFFF), ext_tcode & 0xFF,
		       be32_to_cpu(data), be32_to_cpu(arg));
	}
	spin_lock_irqsave(&host_info_lock, irqflags);
	hi = find_host_info(host);	/* search address-entry */
	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_lock FAILED addr_entry not found"
		       " -> rcode_address_error\n");
		spin_unlock_irqrestore(&host_info_lock, irqflags);
		return (RCODE_ADDRESS_ERROR);
	} else {
		DBGMSG("arm_lock 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_be32(be32_to_cpu(data) +
							be32_to_cpu(old));
					break;
				case (EXTCODE_LITTLE_ADD):
					new =
					    cpu_to_le32(le32_to_cpu(data) +
							le32_to_cpu(old));
					break;
				case (EXTCODE_BOUNDED_ADD):
					if (old != arg) {
						new =
						    cpu_to_be32(be32_to_cpu
								(data) +
								be32_to_cpu
								(old));
					} else {
						new = old;
					}
					break;
				case (EXTCODE_WRAP_ADD):
					if (old != arg) {
						new =
						    cpu_to_be32(be32_to_cpu
								(data) +
								be32_to_cpu
								(old));
					} else {
						new = data;
					}
					break;
				default:
					rcode = RCODE_TYPE_ERROR;	/* function not allowed */
					printk(KERN_ERR
					       "raw1394: arm_lock FAILED "
					       "ext_tcode not allowed -> rcode_type_error\n");
					break;
				}	/*switch */
				if (rcode == -1) {
					DBGMSG("arm_lock -> (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_lock -> rcode_type_error (access denied)");
		}
	}
	if (arm_addr->notification_options & ARM_LOCK) {
		byte_t *buf1, *buf2;
		DBGMSG("arm_lock -> entering notification-section");
		req = __alloc_pending_request(GFP_ATOMIC);
		if (!req) {
			DBGMSG("arm_lock -> rcode_conflict_error");
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			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);
			DBGMSG("arm_lock -> rcode_conflict_error");
			spin_unlock_irqrestore(&host_info_lock, irqflags);
			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);
}