raw1394.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 1,567 行 · 第 1/5 页

                        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(&host_info_lock);
        return(rcode);
}

static int arm_write (struct hpsb_host *host, int nodeid, int destid,
		      quadlet_t *data, u64 addr, size_t length, u16 flags)
{
        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(&host_info_lock);
        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(&host_info_lock);
                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(SLAB_ATOMIC);
                if (!req) {
                        DBGMSG("arm_write -> rcode_conflict_error");
                        spin_unlock(&host_info_lock);
                        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, SLAB_ATOMIC);
                if (!(req->data)) {
                        free_pending_request(req);
                        DBGMSG("arm_write -> rcode_conflict_error");
                        spin_unlock(&host_info_lock);
                        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(&host_info_lock);
        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)
{
        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(&host_info_lock);
        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(&host_info_lock);
                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(SLAB_ATOMIC);
                if (!req) {
                        DBGMSG("arm_lock -> rcode_conflict_error");
                        spin_unlock(&host_info_lock);
                        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);
                        DBGMSG("arm_lock -> rcode_conflict_error");
                        spin_unlock(&host_info_lock);
                        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);