16550a.c

xenomai 很好的linux实时补丁

    size_t                  read = 0;
    int                     pending;
    int                     block;
    int                     subblock;
    int                     in_pos;
    char                    *out_pos = (char *)buf;
    rtdm_toseq_t            timeout_seq;
    ssize_t                 ret = -EAGAIN;  /* for non-blocking read */
    int                     nonblocking;


    if (nbyte == 0)
        return 0;

    if (user_info && !rtdm_rw_user_ok(user_info, buf, nbyte))
        return -EFAULT;

    ctx    = (struct rt_16550_context *)context->dev_private;
    dev_id = ctx->dev_id;

    rtdm_toseq_init(&timeout_seq, ctx->config.rx_timeout);

    /* non-blocking is handled separately here */
    nonblocking = (ctx->config.rx_timeout < 0);

    /* only one reader allowed, stop any further attempts here */
    if (test_and_set_bit(0, &ctx->in_lock))
        return -EBUSY;

    while (nbyte > 0) {
        rtdm_lock_get_irqsave(&ctx->lock, lock_ctx);

        /* switch on error interrupt - the user is ready to listen */
        if (!testbits(ctx->ier_status, IER_STAT)) {
            ctx->ier_status |= IER_STAT;
            outb(ctx->ier_status, IER(ctx->dev_id));
        }

        if (ctx->status) {
            if (testbits(ctx->status, RTSER_LSR_BREAK_IND))
                ret = -EPIPE;
            else
                ret = -EIO;
            ctx->status = 0;
            break;
        }

        pending = ctx->in_npend;

        if (pending > 0) {
            block = subblock = (pending <= nbyte) ? pending : nbyte;
            in_pos = ctx->in_head;

            rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);

            /* do we have to wrap around the buffer end? */
            if (in_pos + subblock > IN_BUFFER_SIZE) {
                /* treat the block between head and buffer end separately */
                subblock = IN_BUFFER_SIZE - in_pos;

                if (user_info) {
                    if (rtdm_copy_to_user(user_info, out_pos,
                                &ctx->in_buf[in_pos], subblock) != 0) {
                        ret = -EFAULT;
                        break;
                    }
                } else
                    memcpy(out_pos, &ctx->in_buf[in_pos], subblock);

                read    += subblock;
                out_pos += subblock;

                subblock = block - subblock;
                in_pos   = 0;
            }

            if (user_info) {
                if (rtdm_copy_to_user(user_info, out_pos,
                                &ctx->in_buf[in_pos], subblock) != 0) {
                    ret = -EFAULT;
                    break;
                }
            } else
                memcpy(out_pos, &ctx->in_buf[in_pos], subblock);

            read    += subblock;
            out_pos += subblock;
            nbyte   -= block;

            rtdm_lock_get_irqsave(&ctx->lock, lock_ctx);

            ctx->in_head = (ctx->in_head + block) & (IN_BUFFER_SIZE - 1);
            if ((ctx->in_npend -= block) == 0)
                ctx->ioc_events &= ~RTSER_EVENT_RXPEND;

            rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);
            continue;
        }

        if (nonblocking) {
            rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);
            /* ret was set to EAGAIN in case of real non-blocking call or
               contains the error returned by rtdm_event_wait[_until] */
            break;
        }

        ctx->in_nwait = nbyte;

        rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);

        ret = rtdm_event_timedwait(&ctx->in_event, ctx->config.rx_timeout,
                                   &timeout_seq);
        if (ret < 0) {
            if (ret == -EIDRM) {
                /* device has been closed - return immediately */
                return -EBADF;
            }

            nonblocking = 1;
            if (ctx->in_npend > 0)
                continue; /* final turn: collect pending bytes before exit */

            ctx->in_nwait = 0;
            break;
        }
    }

    /* release the simple reader lock */
    clear_bit(0, &ctx->in_lock);

    if ((read > 0) && ((ret == 0) || (ret == -EAGAIN) ||
                       (ret == -ETIMEDOUT) || (ret == -EINTR)))
        ret = read;

    return ret;
}


ssize_t rt_16550_write(struct rtdm_dev_context *context,
                       rtdm_user_info_t *user_info, const void *buf,
                       size_t nbyte)
{
    struct rt_16550_context *ctx;
    int                     dev_id;
    rtdm_lockctx_t          lock_ctx;
    size_t                  written = 0;
    int                     free;
    int                     block;
    int                     subblock;
    int                     out_pos;
    char                    *in_pos = (char *)buf;
    rtdm_toseq_t            timeout_seq;
    ssize_t                 ret;


    if (nbyte == 0)
        return 0;

    if (user_info &&
        !rtdm_read_user_ok(user_info, buf, nbyte))
        return -EFAULT;

    ctx    = (struct rt_16550_context *)context->dev_private;
    dev_id = ctx->dev_id;

    rtdm_toseq_init(&timeout_seq, ctx->config.rx_timeout);

    /* make write operation atomic */
    ret = rtdm_mutex_timedlock(&ctx->out_lock, ctx->config.rx_timeout,
                               &timeout_seq);
    if (ret)
        return ret;

    while (nbyte > 0) {
        rtdm_lock_get_irqsave(&ctx->lock, lock_ctx);

        free = OUT_BUFFER_SIZE - ctx->out_npend;

        if (free > 0) {
            block = subblock = (nbyte <= free) ? nbyte : free;
            out_pos = ctx->out_tail;

            rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);

            /* do we have to wrap around the buffer end? */
            if (out_pos + subblock > OUT_BUFFER_SIZE) {
                /* treat the block between head and buffer end separately */
                subblock = OUT_BUFFER_SIZE - out_pos;

                if (user_info) {
                    if (rtdm_copy_from_user(user_info, &ctx->out_buf[out_pos],
                                            in_pos, subblock) != 0) {
                        ret = -EFAULT;
                        break;
                    }
                } else
                    memcpy(&ctx->out_buf[out_pos], in_pos, subblock);

                written += subblock;
                in_pos  += subblock;

                subblock = block - subblock;
                out_pos  = 0;
            }

            if (user_info) {
                if (rtdm_copy_from_user(user_info, &ctx->out_buf[out_pos],
                                        in_pos, subblock) != 0) {
                    ret = -EFAULT;
                    break;
                }
            } else
                memcpy(&ctx->out_buf[out_pos], in_pos, block);

            written += subblock;
            in_pos  += subblock;
            nbyte   -= block;

            rtdm_lock_get_irqsave(&ctx->lock, lock_ctx);

            ctx->out_tail = (ctx->out_tail + block) & (OUT_BUFFER_SIZE - 1);
            ctx->out_npend += block;

            /* unmask tx interrupt */
            ctx->ier_status |= IER_TX;
            outb(ctx->ier_status, IER(dev_id));

            rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);
            continue;
        }

        rtdm_lock_put_irqrestore(&ctx->lock, lock_ctx);

        ret = rtdm_event_timedwait(&ctx->out_event, ctx->config.tx_timeout,
                                   &timeout_seq);
        if (ret < 0) {
            if (ret == -EIDRM) {
                /* device has been closed - return immediately */
                return -EBADF;
            }
            if (ret == -EWOULDBLOCK) {
                /* fix error code for non-blocking mode */
                ret = -EAGAIN;
            }
            break;
        }
    }

    rtdm_mutex_unlock(&ctx->out_lock);

    if ((written > 0) && ((ret == 0) || (ret == -EAGAIN) ||
                          (ret == -ETIMEDOUT) || (ret == -EINTR)))
        ret = written;

    return ret;
}


static const struct rtdm_device __initdata device_tmpl = {
    struct_version:     RTDM_DEVICE_STRUCT_VER,

    device_flags:       RTDM_NAMED_DEVICE | RTDM_EXCLUSIVE,
    context_size:       sizeof(struct rt_16550_context),
    device_name:        "",

    open_rt:            rt_16550_open,
    open_nrt:           rt_16550_open,

    ops: {
        close_rt:       rt_16550_close,
        close_nrt:      rt_16550_close,

        ioctl_rt:       rt_16550_ioctl,
        ioctl_nrt:      rt_16550_ioctl,

        read_rt:        rt_16550_read,
        read_nrt:       NULL,

        write_rt:       rt_16550_write,
        write_nrt:      NULL,

        recvmsg_rt:     NULL,
        recvmsg_nrt:    NULL,

        sendmsg_rt:     NULL,
        sendmsg_nrt:    NULL,
    },

    device_class:       RTDM_CLASS_SERIAL,
    device_sub_class:   RTDM_SUBCLASS_16550A,
    driver_name:        "xeno_16550A",
    driver_version:     RTDM_DRIVER_VER(1, 2, 6),
    peripheral_name:    "UART 16550A",
    provider_name:      "Jan Kiszka",
};

void __16550A_exit(void);

int __16550A_init(void)
{
    struct rtdm_device  *dev;
    int                 ret;
    int                 i;


    for (i = 0; i < MAX_DEVICES; i++) {
        if (!ioaddr[i])
            continue;

        ret = -EINVAL;
        if (!irq[i]) {
            goto cleanup_out;
        }

        dev = kmalloc(sizeof(struct rtdm_device), GFP_KERNEL);
        ret = -ENOMEM;
        if (!dev)
            goto cleanup_out;

        memcpy(dev, &device_tmpl, sizeof(struct rtdm_device));
        snprintf(dev->device_name, RTDM_MAX_DEVNAME_LEN, "rtser%d",
                 start_index+i);
        dev->device_id = i;

        dev->proc_name = dev->device_name;

        ret = -EBUSY;
        if (!request_region(ioaddr[i], 8, dev->device_name))
            goto kfree_out;

        if (baud_base[i] == 0)
            baud_base[i] = DEFAULT_BAUD_BASE;

        if (tx_fifo[i] == 0)
            tx_fifo[i] = DEFAULT_TX_FIFO;

        /* Mask all UART interrupts and clear pending ones. */
        outb(0, IER(i));
        inb(IIR(i));
        inb(LSR(i));
        inb(RHR(i));
        inb(MSR(i));

        ret = rtdm_dev_register(dev);

        if (ret < 0)
            goto rel_region_out;

        device[i] = dev;
    }

    return 0;


 rel_region_out:
    release_region(ioaddr[i], 8);

 kfree_out:
    kfree(dev);

 cleanup_out:
    __16550A_exit();

    return ret;
}


void __16550A_exit(void)
{
    int i;


    for (i = 0; i < MAX_DEVICES; i++)
        if (device[i]) {
            rtdm_dev_unregister(device[i], 1000);
            release_region(ioaddr[i], 8);
            kfree(device[i]);
        }
}

module_init(__16550A_init);
module_exit(__16550A_exit);