raw1394.c

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

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

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)
{
        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(&host_info_lock);
        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(&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) {
		byte_t *buf1, *buf2;
                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)));
                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(&host_info_lock);
        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 = (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