drivers/rpmsg/qcom_glink_rpm.c

   1 /*
   2  * Copyright (c) 2016-2017, Linaro Ltd
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License version 2 and
   6  * only version 2 as published by the Free Software Foundation.
   7  *
   8  * This program is distributed in the hope that it will be useful,
   9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  11  * GNU General Public License for more details.
  12  */
  13
  14 #include <linux/idr.h>
  15 #include <linux/interrupt.h>
  16 #include <linux/io.h>
  17 #include <linux/list.h>
  18 #include <linux/mfd/syscon.h>
  19 #include <linux/module.h>
  20 #include <linux/of.h>
  21 #include <linux/of_address.h>
  22 #include <linux/platform_device.h>
  23 #include <linux/regmap.h>
  24 #include <linux/rpmsg.h>
  25 #include <linux/slab.h>
  26 #include <linux/workqueue.h>
  27 #include <linux/mailbox_client.h>
  28
  29 #include "rpmsg_internal.h"
  30
  31 #define RPM_TOC_SIZE            256
  32 #define RPM_TOC_MAGIC           0x67727430 /* grt0 */
  33 #define RPM_TOC_MAX_ENTRIES     ((RPM_TOC_SIZE - sizeof(struct rpm_toc)) / \
  34                                  sizeof(struct rpm_toc_entry))
  35
  36 #define RPM_TX_FIFO_ID          0x61703272 /* ap2r */
  37 #define RPM_RX_FIFO_ID          0x72326170 /* r2ap */
  38
  39 #define GLINK_NAME_SIZE         32
  40
  41 #define RPM_GLINK_CID_MIN       1
  42 #define RPM_GLINK_CID_MAX       65536
  43
  44 struct rpm_toc_entry {
  45         __le32 id;
  46         __le32 offset;
  47         __le32 size;
  48 } __packed;
  49
  50 struct rpm_toc {
  51         __le32 magic;
  52         __le32 count;
  53
  54         struct rpm_toc_entry entries[];
  55 } __packed;
  56
  57 struct glink_msg {
  58         __le16 cmd;
  59         __le16 param1;
  60         __le32 param2;
  61         u8 data[];
  62 } __packed;
  63
  64 struct glink_rpm_pipe {
  65         void __iomem *tail;
  66         void __iomem *head;
  67
  68         void __iomem *fifo;
  69
  70         size_t length;
  71 };
  72
  73 /**
  74  * struct glink_defer_cmd - deferred incoming control message
  75  * @node:       list node
  76  * @msg:        message header
  77  * data:        payload of the message
  78  *
  79  * Copy of a received control message, to be added to @rx_queue and processed
  80  * by @rx_work of @glink_rpm.
  81  */
  82 struct glink_defer_cmd {
  83         struct list_head node;
  84
  85         struct glink_msg msg;
  86         u8 data[];
  87 };
  88
  89 /**
  90  * struct glink_rpm - driver context, relates to one remote subsystem
  91  * @dev:        reference to the associated struct device
  92  * @doorbell:   "rpm_hlos" ipc doorbell
  93  * @rx_pipe:    pipe object for receive FIFO
  94  * @tx_pipe:    pipe object for transmit FIFO
  95  * @irq:        IRQ for signaling incoming events
  96  * @rx_work:    worker for handling received control messages
  97  * @rx_lock:    protects the @rx_queue
  98  * @rx_queue:   queue of received control messages to be processed in @rx_work
  99  * @tx_lock:    synchronizes operations on the tx fifo
 100  * @idr_lock:   synchronizes @lcids and @rcids modifications
 101  * @lcids:      idr of all channels with a known local channel id
 102  * @rcids:      idr of all channels with a known remote channel id
 103  */
 104 struct qcom_glink {
 105         struct device *dev;
 106
 107         struct mbox_client mbox_client;
 108         struct mbox_chan *mbox_chan;
 109
 110         struct glink_rpm_pipe rx_pipe;
 111         struct glink_rpm_pipe tx_pipe;
 112
 113         int irq;
 114
 115         struct work_struct rx_work;
 116         spinlock_t rx_lock;
 117         struct list_head rx_queue;
 118
 119         struct mutex tx_lock;
 120
 121         struct mutex idr_lock;
 122         struct idr lcids;
 123         struct idr rcids;
 124 };
 125
 126 enum {
 127         GLINK_STATE_CLOSED,
 128         GLINK_STATE_OPENING,
 129         GLINK_STATE_OPEN,
 130         GLINK_STATE_CLOSING,
 131 };
 132
 133 /**
 134  * struct glink_channel - internal representation of a channel
 135  * @rpdev:      rpdev reference, only used for primary endpoints
 136  * @ept:        rpmsg endpoint this channel is associated with
 137  * @glink:      qcom_glink context handle
 138  * @refcount:   refcount for the channel object
 139  * @recv_lock:  guard for @ept.cb
 140  * @name:       unique channel name/identifier
 141  * @lcid:       channel id, in local space
 142  * @rcid:       channel id, in remote space
 143  * @buf:        receive buffer, for gathering fragments
 144  * @buf_offset: write offset in @buf
 145  * @buf_size:   size of current @buf
 146  * @open_ack:   completed once remote has acked the open-request
 147  * @open_req:   completed once open-request has been received
 148  */
 149 struct glink_channel {
 150         struct rpmsg_endpoint ept;
 151
 152         struct rpmsg_device *rpdev;
 153         struct qcom_glink *glink;
 154
 155         struct kref refcount;
 156
 157         spinlock_t recv_lock;
 158
 159         char *name;
 160         unsigned int lcid;
 161         unsigned int rcid;
 162
 163         void *buf;
 164         int buf_offset;
 165         int buf_size;
 166
 167         struct completion open_ack;
 168         struct completion open_req;
 169 };
 170
 171 #define to_glink_channel(_ept) container_of(_ept, struct glink_channel, ept)
 172
 173 static const struct rpmsg_endpoint_ops glink_endpoint_ops;
 174
 175 #define RPM_CMD_VERSION                 0
 176 #define RPM_CMD_VERSION_ACK             1
 177 #define RPM_CMD_OPEN                    2
 178 #define RPM_CMD_CLOSE                   3
 179 #define RPM_CMD_OPEN_ACK                4
 180 #define RPM_CMD_TX_DATA                 9
 181 #define RPM_CMD_CLOSE_ACK               11
 182 #define RPM_CMD_TX_DATA_CONT            12
 183 #define RPM_CMD_READ_NOTIF              13
 184
 185 #define GLINK_FEATURE_INTENTLESS        BIT(1)
 186
 187 static struct glink_channel *qcom_glink_alloc_channel(struct qcom_glink *glink,
 188                                                       const char *name)
 189 {
 190         struct glink_channel *channel;
 191
 192         channel = kzalloc(sizeof(*channel), GFP_KERNEL);
 193         if (!channel)
 194                 return ERR_PTR(-ENOMEM);
 195
 196         /* Setup glink internal glink_channel data */
 197         spin_lock_init(&channel->recv_lock);
 198         channel->glink = glink;
 199         channel->name = kstrdup(name, GFP_KERNEL);
 200
 201         init_completion(&channel->open_req);
 202         init_completion(&channel->open_ack);
 203
 204         kref_init(&channel->refcount);
 205
 206         return channel;
 207 }
 208
 209 static void qcom_glink_channel_release(struct kref *ref)
 210 {
 211         struct glink_channel *channel = container_of(ref, struct glink_channel,
 212                                                      refcount);
 213
 214         kfree(channel->name);
 215         kfree(channel);
 216 }
 217
 218 static size_t qcom_glink_rx_avail(struct qcom_glink *glink)
 219 {
 220         struct glink_rpm_pipe *pipe = &glink->rx_pipe;
 221         unsigned int head;
 222         unsigned int tail;
 223
 224         head = readl(pipe->head);
 225         tail = readl(pipe->tail);
 226
 227         if (head < tail)
 228                 return pipe->length - tail + head;
 229         else
 230                 return head - tail;
 231 }
 232
 233 static void qcom_glink_rx_peak(struct qcom_glink *glink,
 234                                void *data, size_t count)
 235 {
 236         struct glink_rpm_pipe *pipe = &glink->rx_pipe;
 237         unsigned int tail;
 238         size_t len;
 239
 240         tail = readl(pipe->tail);
 241
 242         len = min_t(size_t, count, pipe->length - tail);
 243         if (len) {
 244                 __ioread32_copy(data, pipe->fifo + tail,
 245                                 len / sizeof(u32));
 246         }
 247
 248         if (len != count) {
 249                 __ioread32_copy(data + len, pipe->fifo,
 250                                 (count - len) / sizeof(u32));
 251         }
 252 }
 253
 254 static void qcom_glink_rx_advance(struct qcom_glink *glink,
 255                                   size_t count)
 256 {
 257         struct glink_rpm_pipe *pipe = &glink->rx_pipe;
 258         unsigned int tail;
 259
 260         tail = readl(pipe->tail);
 261
 262         tail += count;
 263         if (tail >= pipe->length)
 264                 tail -= pipe->length;
 265
 266         writel(tail, pipe->tail);
 267 }
 268
 269 static size_t qcom_glink_tx_avail(struct qcom_glink *glink)
 270 {
 271         struct glink_rpm_pipe *pipe = &glink->tx_pipe;
 272         unsigned int head;
 273         unsigned int tail;
 274
 275         head = readl(pipe->head);
 276         tail = readl(pipe->tail);
 277
 278         if (tail <= head)
 279                 return pipe->length - head + tail;
 280         else
 281                 return tail - head;
 282 }
 283
 284 static unsigned int qcom_glink_tx_write(struct qcom_glink *glink,
 285                                         unsigned int head,
 286                                         const void *data, size_t count)
 287 {
 288         struct glink_rpm_pipe *pipe = &glink->tx_pipe;
 289         size_t len;
 290
 291         len = min_t(size_t, count, pipe->length - head);
 292         if (len) {
 293                 __iowrite32_copy(pipe->fifo + head, data,
 294                                  len / sizeof(u32));
 295         }
 296
 297         if (len != count) {
 298                 __iowrite32_copy(pipe->fifo, data + len,
 299                                  (count - len) / sizeof(u32));
 300         }
 301
 302         head += count;
 303         if (head >= pipe->length)
 304                 head -= pipe->length;
 305
 306         return head;
 307 }
 308
 309 static int qcom_glink_tx(struct qcom_glink *glink,
 310                          const void *hdr, size_t hlen,
 311                         const void *data, size_t dlen, bool wait)
 312 {
 313         struct glink_rpm_pipe *pipe = &glink->tx_pipe;
 314         unsigned int head;
 315         unsigned int tlen = hlen + dlen;
 316         int ret;
 317
 318         /* Reject packets that are too big */
 319         if (tlen >= glink->tx_pipe.length)
 320                 return -EINVAL;
 321
 322         if (WARN(tlen % 8, "Unaligned TX request"))
 323                 return -EINVAL;
 324
 325         ret = mutex_lock_interruptible(&glink->tx_lock);
 326         if (ret)
 327                 return ret;
 328
 329         while (qcom_glink_tx_avail(glink) < tlen) {
 330                 if (!wait) {
 331                         ret = -ENOMEM;
 332                         goto out;
 333                 }
 334
 335                 msleep(10);
 336         }
 337
 338         head = readl(pipe->head);
 339         head = qcom_glink_tx_write(glink, head, hdr, hlen);
 340         head = qcom_glink_tx_write(glink, head, data, dlen);
 341         writel(head, pipe->head);
 342
 343         mbox_send_message(glink->mbox_chan, NULL);
 344         mbox_client_txdone(glink->mbox_chan, 0);
 345
 346 out:
 347         mutex_unlock(&glink->tx_lock);
 348
 349         return ret;
 350 }
 351
 352 static int qcom_glink_send_version(struct qcom_glink *glink)
 353 {
 354         struct glink_msg msg;
 355
 356         msg.cmd = cpu_to_le16(RPM_CMD_VERSION);
 357         msg.param1 = cpu_to_le16(1);
 358         msg.param2 = cpu_to_le32(GLINK_FEATURE_INTENTLESS);
 359
 360         return qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true);
 361 }
 362
 363 static void qcom_glink_send_version_ack(struct qcom_glink *glink)
 364 {
 365         struct glink_msg msg;
 366
 367         msg.cmd = cpu_to_le16(RPM_CMD_VERSION_ACK);
 368         msg.param1 = cpu_to_le16(1);
 369         msg.param2 = cpu_to_le32(0);
 370
 371         qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true);
 372 }
 373
 374 static void qcom_glink_send_open_ack(struct qcom_glink *glink,
 375                                      struct glink_channel *channel)
 376 {
 377         struct glink_msg msg;
 378
 379         msg.cmd = cpu_to_le16(RPM_CMD_OPEN_ACK);
 380         msg.param1 = cpu_to_le16(channel->rcid);
 381         msg.param2 = cpu_to_le32(0);
 382
 383         qcom_glink_tx(glink, &msg, sizeof(msg), NULL, 0, true);
 384 }
 385
 386 /**
 387  * qcom_glink_send_open_req() - send a RPM_CMD_OPEN request to the remote
 388  * @glink:
 389  * @channel:
 390  *
 391  * Allocates a local channel id and sends a RPM_CMD_OPEN message to the remote.
 392  * Will return with refcount held, regardless of outcome.
 393  *
 394  * Returns 0 on success, negative errno otherwise.
 395  */
 396 static int qcom_glink_send_open_req(struct qcom_glink *glink,
 397                                     struct glink_channel *channel)
 398 {
 399         struct {
 400                 struct glink_msg msg;
 401                 u8 name[GLINK_NAME_SIZE];
 402         } __packed req;
 403         int name_len = strlen(channel->name) + 1;
 404         int req_len = ALIGN(sizeof(req.msg) + name_len, 8);
 405         int ret;
 406
 407         kref_get(&channel->refcount);
 408
 409         mutex_lock(&glink->idr_lock);
 410         ret = idr_alloc_cyclic(&glink->lcids, channel,
 411                                RPM_GLINK_CID_MIN, RPM_GLINK_CID_MAX, GFP_KERNEL);
 412         mutex_unlock(&glink->idr_lock);
 413         if (ret < 0)
 414                 return ret;
 415
 416         channel->lcid = ret;
 417
 418         req.msg.cmd = cpu_to_le16(RPM_CMD_OPEN);
 419         req.msg.param1 = cpu_to_le16(channel->lcid);
 420         req.msg.param2 = cpu_to_le32(name_len);
 421         strcpy(req.name, channel->name);
 422
 423         ret = qcom_glink_tx(glink, &req, req_len, NULL, 0, true);
 424         if (ret)
 425                 goto remove_idr;
 426
 427         return 0;
 428
 429 remove_idr:
 430         mutex_lock(&glink->idr_lock);
 431         idr_remove(&glink->lcids, channel->lcid);
 432         channel->lcid = 0;
 433         mutex_unlock(&glink->idr_lock);
 434
 435         return ret;
 436 }
 437
 438 static void qcom_glink_send_close_req(struct qcom_glink *glink,
 439                                       struct glink_channel *channel)
 440 {
 441         struct glink_msg req;
 442
 443         req.cmd = cpu_to_le16(RPM_CMD_CLOSE);
 444         req.param1 = cpu_to_le16(channel->lcid);
 445         req.param2 = 0;
 446
 447         qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true);
 448 }
 449
 450 static void qcom_glink_send_close_ack(struct qcom_glink *glink,
 451                                       unsigned int rcid)
 452 {
 453         struct glink_msg req;
 454
 455         req.cmd = cpu_to_le16(RPM_CMD_CLOSE_ACK);
 456         req.param1 = cpu_to_le16(rcid);
 457         req.param2 = 0;
 458
 459         qcom_glink_tx(glink, &req, sizeof(req), NULL, 0, true);
 460 }
 461
 462 static int qcom_glink_rx_defer(struct qcom_glink *glink, size_t extra)
 463 {
 464         struct glink_defer_cmd *dcmd;
 465
 466         extra = ALIGN(extra, 8);
 467
 468         if (qcom_glink_rx_avail(glink) < sizeof(struct glink_msg) + extra) {
 469                 dev_dbg(glink->dev, "Insufficient data in rx fifo");
 470                 return -ENXIO;
 471         }
 472
 473         dcmd = kzalloc(sizeof(*dcmd) + extra, GFP_ATOMIC);
 474         if (!dcmd)
 475                 return -ENOMEM;
 476
 477         INIT_LIST_HEAD(&dcmd->node);
 478
 479         qcom_glink_rx_peak(glink, &dcmd->msg, sizeof(dcmd->msg) + extra);
 480
 481         spin_lock(&glink->rx_lock);
 482         list_add_tail(&dcmd->node, &glink->rx_queue);
 483         spin_unlock(&glink->rx_lock);
 484
 485         schedule_work(&glink->rx_work);
 486         qcom_glink_rx_advance(glink, sizeof(dcmd->msg) + extra);
 487
 488         return 0;
 489 }
 490
 491 static int qcom_glink_rx_data(struct qcom_glink *glink, size_t avail)
 492 {
 493         struct glink_channel *channel;
 494         struct {
 495                 struct glink_msg msg;
 496                 __le32 chunk_size;
 497                 __le32 left_size;
 498         } __packed hdr;
 499         unsigned int chunk_size;
 500         unsigned int left_size;
 501         unsigned int rcid;
 502
 503         if (avail < sizeof(hdr)) {
 504                 dev_dbg(glink->dev, "Not enough data in fifo\n");
 505                 return -EAGAIN;
 506         }
 507
 508         qcom_glink_rx_peak(glink, &hdr, sizeof(hdr));
 509         chunk_size = le32_to_cpu(hdr.chunk_size);
 510         left_size = le32_to_cpu(hdr.left_size);
 511
 512         if (avail < sizeof(hdr) + chunk_size) {
 513                 dev_dbg(glink->dev, "Payload not yet in fifo\n");
 514                 return -EAGAIN;
 515         }
 516
 517         if (WARN(chunk_size % 4, "Incoming data must be word aligned\n"))
 518                 return -EINVAL;
 519
 520         rcid = le16_to_cpu(hdr.msg.param1);
 521         channel = idr_find(&glink->rcids, rcid);
 522         if (!channel) {
 523                 dev_dbg(glink->dev, "Data on non-existing channel\n");
 524
 525                 /* Drop the message */
 526                 qcom_glink_rx_advance(glink,
 527                                       ALIGN(sizeof(hdr) + chunk_size, 8));
 528                 return 0;
 529         }
 530
 531         /* Might have an ongoing, fragmented, message to append */
 532         if (!channel->buf) {
 533                 channel->buf = kmalloc(chunk_size + left_size, GFP_ATOMIC);
 534                 if (!channel->buf)
 535                         return -ENOMEM;
 536
 537                 channel->buf_size = chunk_size + left_size;
 538                 channel->buf_offset = 0;
 539         }
 540
 541         qcom_glink_rx_advance(glink, sizeof(hdr));
 542
 543         if (channel->buf_size - channel->buf_offset < chunk_size) {
 544                 dev_err(glink->dev, "Insufficient space in input buffer\n");
 545
 546                 /* The packet header lied, drop payload */
 547                 qcom_glink_rx_advance(glink, chunk_size);
 548                 return -ENOMEM;
 549         }
 550
 551         qcom_glink_rx_peak(glink, channel->buf + channel->buf_offset,
 552                            chunk_size);
 553         channel->buf_offset += chunk_size;
 554
 555         /* Handle message when no fragments remain to be received */
 556         if (!left_size) {
 557                 spin_lock(&channel->recv_lock);
 558                 if (channel->ept.cb) {
 559                         channel->ept.cb(channel->ept.rpdev,
 560                                         channel->buf,
 561                                         channel->buf_offset,
 562                                         channel->ept.priv,
 563                                         RPMSG_ADDR_ANY);
 564                 }
 565                 spin_unlock(&channel->recv_lock);
 566
 567                 kfree(channel->buf);
 568                 channel->buf = NULL;
 569                 channel->buf_size = 0;
 570         }
 571
 572         /* Each message starts at 8 byte aligned address */
 573         qcom_glink_rx_advance(glink, ALIGN(chunk_size, 8));
 574
 575         return 0;
 576 }
 577
 578 static int qcom_glink_rx_open_ack(struct qcom_glink *glink, unsigned int lcid)
 579 {
 580         struct glink_channel *channel;
 581
 582         channel = idr_find(&glink->lcids, lcid);
 583         if (!channel) {
 584                 dev_err(glink->dev, "Invalid open ack packet\n");
 585                 return -EINVAL;
 586         }
 587
 588         complete(&channel->open_ack);
 589
 590         return 0;
 591 }
 592
 593 static irqreturn_t qcom_glink_intr(int irq, void *data)
 594 {
 595         struct qcom_glink *glink = data;
 596         struct glink_msg msg;
 597         unsigned int param1;
 598         unsigned int param2;
 599         unsigned int avail;
 600         unsigned int cmd;
 601         int ret;
 602
 603         for (;;) {
 604                 avail = qcom_glink_rx_avail(glink);
 605                 if (avail < sizeof(msg))
 606                         break;
 607
 608                 qcom_glink_rx_peak(glink, &msg, sizeof(msg));
 609
 610                 cmd = le16_to_cpu(msg.cmd);
 611                 param1 = le16_to_cpu(msg.param1);
 612                 param2 = le32_to_cpu(msg.param2);
 613
 614                 switch (cmd) {
 615                 case RPM_CMD_VERSION:
 616                 case RPM_CMD_VERSION_ACK:
 617                 case RPM_CMD_CLOSE:
 618                 case RPM_CMD_CLOSE_ACK:
 619                         ret = qcom_glink_rx_defer(glink, 0);
 620                         break;
 621                 case RPM_CMD_OPEN_ACK:
 622                         ret = qcom_glink_rx_open_ack(glink, param1);
 623                         qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8));
 624                         break;
 625                 case RPM_CMD_OPEN:
 626                         ret = qcom_glink_rx_defer(glink, param2);
 627                         break;
 628                 case RPM_CMD_TX_DATA:
 629                 case RPM_CMD_TX_DATA_CONT:
 630                         ret = qcom_glink_rx_data(glink, avail);
 631                         break;
 632                 case RPM_CMD_READ_NOTIF:
 633                         qcom_glink_rx_advance(glink, ALIGN(sizeof(msg), 8));
 634
 635                         mbox_send_message(glink->mbox_chan, NULL);
 636                         mbox_client_txdone(glink->mbox_chan, 0);
 637
 638                         ret = 0;
 639                         break;
 640                 default:
 641                         dev_err(glink->dev, "unhandled rx cmd: %d\n", cmd);
 642                         ret = -EINVAL;
 643                         break;
 644                 }
 645
 646                 if (ret)
 647                         break;
 648         }
 649
 650         return IRQ_HANDLED;
 651 }
 652
 653 /* Locally initiated rpmsg_create_ept */
 654 static struct glink_channel *qcom_glink_create_local(struct qcom_glink *glink,
 655                                                      const char *name)
 656 {
 657         struct glink_channel *channel;
 658         int ret;
 659
 660         channel = qcom_glink_alloc_channel(glink, name);
 661         if (IS_ERR(channel))
 662                 return ERR_CAST(channel);
 663
 664         ret = qcom_glink_send_open_req(glink, channel);
 665         if (ret)
 666                 goto release_channel;
 667
 668         ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ);
 669         if (!ret)
 670                 goto err_timeout;
 671
 672         ret = wait_for_completion_timeout(&channel->open_req, 5 * HZ);
 673         if (!ret)
 674                 goto err_timeout;
 675
 676         qcom_glink_send_open_ack(glink, channel);
 677
 678         return channel;
 679
 680 err_timeout:
 681         /* qcom_glink_send_open_req() did register the channel in lcids*/
 682         mutex_lock(&glink->idr_lock);
 683         idr_remove(&glink->lcids, channel->lcid);
 684         mutex_unlock(&glink->idr_lock);
 685
 686 release_channel:
 687         /* Release qcom_glink_send_open_req() reference */
 688         kref_put(&channel->refcount, qcom_glink_channel_release);
 689         /* Release qcom_glink_alloc_channel() reference */
 690         kref_put(&channel->refcount, qcom_glink_channel_release);
 691
 692         return ERR_PTR(-ETIMEDOUT);
 693 }
 694
 695 /* Remote initiated rpmsg_create_ept */
 696 static int qcom_glink_create_remote(struct qcom_glink *glink,
 697                                     struct glink_channel *channel)
 698 {
 699         int ret;
 700
 701         qcom_glink_send_open_ack(glink, channel);
 702
 703         ret = qcom_glink_send_open_req(glink, channel);
 704         if (ret)
 705                 goto close_link;
 706
 707         ret = wait_for_completion_timeout(&channel->open_ack, 5 * HZ);
 708         if (!ret) {
 709                 ret = -ETIMEDOUT;
 710                 goto close_link;
 711         }
 712
 713         return 0;
 714
 715 close_link:
 716         /*
 717          * Send a close request to "undo" our open-ack. The close-ack will
 718          * release the last reference.
 719          */
 720         qcom_glink_send_close_req(glink, channel);
 721
 722         /* Release qcom_glink_send_open_req() reference */
 723         kref_put(&channel->refcount, qcom_glink_channel_release);
 724
 725         return ret;
 726 }
 727
 728 static struct rpmsg_endpoint *qcom_glink_create_ept(struct rpmsg_device *rpdev,
 729                                                     rpmsg_rx_cb_t cb,
 730                                                     void *priv,
 731                                                     struct rpmsg_channel_info
 732                                                     chinfo)
 733 {
 734         struct glink_channel *parent = to_glink_channel(rpdev->ept);
 735         struct glink_channel *channel;
 736         struct qcom_glink *glink = parent->glink;
 737         struct rpmsg_endpoint *ept;
 738         const char *name = chinfo.name;
 739         int cid;
 740         int ret;
 741
 742         idr_for_each_entry(&glink->rcids, channel, cid) {
 743                 if (!strcmp(channel->name, name))
 744                         break;
 745         }
 746
 747         if (!channel) {
 748                 channel = qcom_glink_create_local(glink, name);
 749                 if (IS_ERR(channel))
 750                         return NULL;
 751         } else {
 752                 ret = qcom_glink_create_remote(glink, channel);
 753                 if (ret)
 754                         return NULL;
 755         }
 756
 757         ept = &channel->ept;
 758         ept->rpdev = rpdev;
 759         ept->cb = cb;
 760         ept->priv = priv;
 761         ept->ops = &glink_endpoint_ops;
 762
 763         return ept;
 764 }
 765
 766 static void qcom_glink_destroy_ept(struct rpmsg_endpoint *ept)
 767 {
 768         struct glink_channel *channel = to_glink_channel(ept);
 769         struct qcom_glink *glink = channel->glink;
 770         unsigned long flags;
 771
 772         spin_lock_irqsave(&channel->recv_lock, flags);
 773         channel->ept.cb = NULL;
 774         spin_unlock_irqrestore(&channel->recv_lock, flags);
 775
 776         /* Decouple the potential rpdev from the channel */
 777         channel->rpdev = NULL;
 778
 779         qcom_glink_send_close_req(glink, channel);
 780 }
 781
 782 static int __qcom_glink_send(struct glink_channel *channel,
 783                              void *data, int len, bool wait)
 784 {
 785         struct qcom_glink *glink = channel->glink;
 786         struct {
 787                 struct glink_msg msg;
 788                 __le32 chunk_size;
 789                 __le32 left_size;
 790         } __packed req;
 791
 792         if (WARN(len % 8, "RPM GLINK expects 8 byte aligned messages\n"))
 793                 return -EINVAL;
 794
 795         req.msg.cmd = cpu_to_le16(RPM_CMD_TX_DATA);
 796         req.msg.param1 = cpu_to_le16(channel->lcid);
 797         req.msg.param2 = cpu_to_le32(channel->rcid);
 798         req.chunk_size = cpu_to_le32(len);
 799         req.left_size = cpu_to_le32(0);
 800
 801         return qcom_glink_tx(glink, &req, sizeof(req), data, len, wait);
 802 }
 803
 804 static int qcom_glink_send(struct rpmsg_endpoint *ept, void *data, int len)
 805 {
 806         struct glink_channel *channel = to_glink_channel(ept);
 807
 808         return __qcom_glink_send(channel, data, len, true);
 809 }
 810
 811 static int qcom_glink_trysend(struct rpmsg_endpoint *ept, void *data, int len)
 812 {
 813         struct glink_channel *channel = to_glink_channel(ept);
 814
 815         return __qcom_glink_send(channel, data, len, false);
 816 }
 817
 818 /*
 819  * Finds the device_node for the glink child interested in this channel.
 820  */
 821 static struct device_node *qcom_glink_match_channel(struct device_node *node,
 822                                                     const char *channel)
 823 {
 824         struct device_node *child;
 825         const char *name;
 826         const char *key;
 827         int ret;
 828
 829         for_each_available_child_of_node(node, child) {
 830                 key = "qcom,glink-channels";
 831                 ret = of_property_read_string(child, key, &name);
 832                 if (ret)
 833                         continue;
 834
 835                 if (strcmp(name, channel) == 0)
 836                         return child;
 837         }
 838
 839         return NULL;
 840 }
 841
 842 static const struct rpmsg_device_ops glink_device_ops = {
 843         .create_ept = qcom_glink_create_ept,
 844 };
 845
 846 static const struct rpmsg_endpoint_ops glink_endpoint_ops = {
 847         .destroy_ept = qcom_glink_destroy_ept,
 848         .send = qcom_glink_send,
 849         .trysend = qcom_glink_trysend,
 850 };
 851
 852 static void qcom_glink_rpdev_release(struct device *dev)
 853 {
 854         struct rpmsg_device *rpdev = to_rpmsg_device(dev);
 855         struct glink_channel *channel = to_glink_channel(rpdev->ept);
 856
 857         channel->rpdev = NULL;
 858         kfree(rpdev);
 859 }
 860
 861 static int qcom_glink_rx_open(struct qcom_glink *glink, unsigned int rcid,
 862                               char *name)
 863 {
 864         struct glink_channel *channel;
 865         struct rpmsg_device *rpdev;
 866         bool create_device = false;
 867         int lcid;
 868         int ret;
 869         struct device_node *node;
 870
 871         idr_for_each_entry(&glink->lcids, channel, lcid) {
 872                 if (!strcmp(channel->name, name))
 873                         break;
 874         }
 875
 876         if (!channel) {
 877                 channel = qcom_glink_alloc_channel(glink, name);
 878                 if (IS_ERR(channel))
 879                         return PTR_ERR(channel);
 880
 881                 /* The opening dance was initiated by the remote */
 882                 create_device = true;
 883         }
 884
 885         mutex_lock(&glink->idr_lock);
 886         ret = idr_alloc(&glink->rcids, channel, rcid, rcid + 1, GFP_KERNEL);
 887         if (ret < 0) {
 888                 dev_err(glink->dev, "Unable to insert channel into rcid list\n");
 889                 mutex_unlock(&glink->idr_lock);
 890                 goto free_channel;
 891         }
 892         channel->rcid = ret;
 893         mutex_unlock(&glink->idr_lock);
 894
 895         complete(&channel->open_req);
 896
 897         if (create_device) {
 898                 rpdev = kzalloc(sizeof(*rpdev), GFP_KERNEL);
 899                 if (!rpdev) {
 900                         ret = -ENOMEM;
 901                         goto rcid_remove;
 902                 }
 903
 904                 rpdev->ept = &channel->ept;
 905                 strncpy(rpdev->id.name, name, RPMSG_NAME_SIZE);
 906                 rpdev->src = RPMSG_ADDR_ANY;
 907                 rpdev->dst = RPMSG_ADDR_ANY;
 908                 rpdev->ops = &glink_device_ops;
 909
 910                 node = qcom_glink_match_channel(glink->dev->of_node, name);
 911                 rpdev->dev.of_node = node;
 912                 rpdev->dev.parent = glink->dev;
 913                 rpdev->dev.release = qcom_glink_rpdev_release;
 914
 915                 ret = rpmsg_register_device(rpdev);
 916                 if (ret)
 917                         goto free_rpdev;
 918
 919                 channel->rpdev = rpdev;
 920         }
 921
 922         return 0;
 923
 924 free_rpdev:
 925         kfree(rpdev);
 926 rcid_remove:
 927         mutex_lock(&glink->idr_lock);
 928         idr_remove(&glink->rcids, channel->rcid);
 929         channel->rcid = 0;
 930         mutex_unlock(&glink->idr_lock);
 931 free_channel:
 932         /* Release the reference, iff we took it */
 933         if (create_device)
 934                 kref_put(&channel->refcount, qcom_glink_channel_release);
 935
 936         return ret;
 937 }
 938
 939 static void qcom_glink_rx_close(struct qcom_glink *glink, unsigned int rcid)
 940 {
 941         struct rpmsg_channel_info chinfo;
 942         struct glink_channel *channel;
 943
 944         channel = idr_find(&glink->rcids, rcid);
 945         if (WARN(!channel, "close request on unknown channel\n"))
 946                 return;
 947
 948         if (channel->rpdev) {
 949                 strncpy(chinfo.name, channel->name, sizeof(chinfo.name));
 950                 chinfo.src = RPMSG_ADDR_ANY;
 951                 chinfo.dst = RPMSG_ADDR_ANY;
 952
 953                 rpmsg_unregister_device(glink->dev, &chinfo);
 954         }
 955
 956         qcom_glink_send_close_ack(glink, channel->rcid);
 957
 958         mutex_lock(&glink->idr_lock);
 959         idr_remove(&glink->rcids, channel->rcid);
 960         channel->rcid = 0;
 961         mutex_unlock(&glink->idr_lock);
 962
 963         kref_put(&channel->refcount, qcom_glink_channel_release);
 964 }
 965
 966 static void qcom_glink_rx_close_ack(struct qcom_glink *glink, unsigned int lcid)
 967 {
 968         struct glink_channel *channel;
 969
 970         channel = idr_find(&glink->lcids, lcid);
 971         if (WARN(!channel, "close ack on unknown channel\n"))
 972                 return;
 973
 974         mutex_lock(&glink->idr_lock);
 975         idr_remove(&glink->lcids, channel->lcid);
 976         channel->lcid = 0;
 977         mutex_unlock(&glink->idr_lock);
 978
 979         kref_put(&channel->refcount, qcom_glink_channel_release);
 980 }
 981
 982 static void qcom_glink_work(struct work_struct *work)
 983 {
 984         struct qcom_glink *glink = container_of(work, struct qcom_glink,
 985                                                 rx_work);
 986         struct glink_defer_cmd *dcmd;
 987         struct glink_msg *msg;
 988         unsigned long flags;
 989         unsigned int param1;
 990         unsigned int param2;
 991         unsigned int cmd;
 992
 993         for (;;) {
 994                 spin_lock_irqsave(&glink->rx_lock, flags);
 995                 if (list_empty(&glink->rx_queue)) {
 996                         spin_unlock_irqrestore(&glink->rx_lock, flags);
 997                         break;
 998                 }
 999                 dcmd = list_first_entry(&glink->rx_queue, struct glink_defer_cmd, node);
1000                 list_del(&dcmd->node);
1001                 spin_unlock_irqrestore(&glink->rx_lock, flags);
1002
1003                 msg = &dcmd->msg;
1004                 cmd = le16_to_cpu(msg->cmd);
1005                 param1 = le16_to_cpu(msg->param1);
1006                 param2 = le32_to_cpu(msg->param2);
1007
1008                 switch (cmd) {
1009                 case RPM_CMD_VERSION:
1010                         qcom_glink_send_version_ack(glink);
1011                         break;
1012                 case RPM_CMD_VERSION_ACK:
1013                         break;
1014                 case RPM_CMD_OPEN:
1015                         qcom_glink_rx_open(glink, param1, msg->data);
1016                         break;
1017                 case RPM_CMD_CLOSE:
1018                         qcom_glink_rx_close(glink, param1);
1019                         break;
1020                 case RPM_CMD_CLOSE_ACK:
1021                         qcom_glink_rx_close_ack(glink, param1);
1022                         break;
1023                 default:
1024                         WARN(1, "Unknown defer object %d\n", cmd);
1025                         break;
1026                 }
1027
1028                 kfree(dcmd);
1029         }
1030 }
1031
1032 static int glink_rpm_parse_toc(struct device *dev,
1033                                void __iomem *msg_ram,
1034                                size_t msg_ram_size,
1035                                struct glink_rpm_pipe *rx,
1036                                struct glink_rpm_pipe *tx)
1037 {
1038         struct rpm_toc *toc;
1039         int num_entries;
1040         unsigned int id;
1041         size_t offset;
1042         size_t size;
1043         void *buf;
1044         int i;
1045
1046         buf = kzalloc(RPM_TOC_SIZE, GFP_KERNEL);
1047         if (!buf)
1048                 return -ENOMEM;
1049
1050         __ioread32_copy(buf, msg_ram + msg_ram_size - RPM_TOC_SIZE,
1051                         RPM_TOC_SIZE / sizeof(u32));
1052
1053         toc = buf;
1054
1055         if (le32_to_cpu(toc->magic) != RPM_TOC_MAGIC) {
1056                 dev_err(dev, "RPM TOC has invalid magic\n");
1057                 goto err_inval;
1058         }
1059
1060         num_entries = le32_to_cpu(toc->count);
1061         if (num_entries > RPM_TOC_MAX_ENTRIES) {
1062                 dev_err(dev, "Invalid number of toc entries\n");
1063                 goto err_inval;
1064         }
1065
1066         for (i = 0; i < num_entries; i++) {
1067                 id = le32_to_cpu(toc->entries[i].id);
1068                 offset = le32_to_cpu(toc->entries[i].offset);
1069                 size = le32_to_cpu(toc->entries[i].size);
1070
1071                 if (offset > msg_ram_size || offset + size > msg_ram_size) {
1072                         dev_err(dev, "TOC entry with invalid size\n");
1073                         continue;
1074                 }
1075
1076                 switch (id) {
1077                 case RPM_RX_FIFO_ID:
1078                         rx->length = size;
1079
1080                         rx->tail = msg_ram + offset;
1081                         rx->head = msg_ram + offset + sizeof(u32);
1082                         rx->fifo = msg_ram + offset + 2 * sizeof(u32);
1083                         break;
1084                 case RPM_TX_FIFO_ID:
1085                         tx->length = size;
1086
1087                         tx->tail = msg_ram + offset;
1088                         tx->head = msg_ram + offset + sizeof(u32);
1089                         tx->fifo = msg_ram + offset + 2 * sizeof(u32);
1090                         break;
1091                 }
1092         }
1093
1094         if (!rx->fifo || !tx->fifo) {
1095                 dev_err(dev, "Unable to find rx and tx descriptors\n");
1096                 goto err_inval;
1097         }
1098
1099         kfree(buf);
1100         return 0;
1101
1102 err_inval:
1103         kfree(buf);
1104         return -EINVAL;
1105 }
1106
1107 static int glink_rpm_probe(struct platform_device *pdev)
1108 {
1109         struct qcom_glink *glink;
1110         struct device_node *np;
1111         void __iomem *msg_ram;
1112         size_t msg_ram_size;
1113         struct device *dev = &pdev->dev;
1114         struct resource r;
1115         int irq;
1116         int ret;
1117
1118         glink = devm_kzalloc(dev, sizeof(*glink), GFP_KERNEL);
1119         if (!glink)
1120                 return -ENOMEM;
1121
1122         glink->dev = dev;
1123
1124         mutex_init(&glink->tx_lock);
1125         spin_lock_init(&glink->rx_lock);
1126         INIT_LIST_HEAD(&glink->rx_queue);
1127         INIT_WORK(&glink->rx_work, qcom_glink_work);
1128
1129         mutex_init(&glink->idr_lock);
1130         idr_init(&glink->lcids);
1131         idr_init(&glink->rcids);
1132
1133         glink->mbox_client.dev = &pdev->dev;
1134         glink->mbox_chan = mbox_request_channel(&glink->mbox_client, 0);
1135         if (IS_ERR(glink->mbox_chan)) {
1136                 if (PTR_ERR(glink->mbox_chan) != -EPROBE_DEFER)
1137                         dev_err(&pdev->dev, "failed to acquire IPC channel\n");
1138                 return PTR_ERR(glink->mbox_chan);
1139         }
1140
1141         np = of_parse_phandle(dev->of_node, "qcom,rpm-msg-ram", 0);
1142         ret = of_address_to_resource(np, 0, &r);
1143         of_node_put(np);
1144         if (ret)
1145                 return ret;
1146
1147         msg_ram = devm_ioremap(dev, r.start, resource_size(&r));
1148         msg_ram_size = resource_size(&r);
1149         if (!msg_ram)
1150                 return -ENOMEM;
1151
1152         ret = glink_rpm_parse_toc(dev, msg_ram, msg_ram_size,
1153                                   &glink->rx_pipe, &glink->tx_pipe);
1154         if (ret)
1155                 return ret;
1156
1157         writel(0, glink->tx_pipe.head);
1158         writel(0, glink->rx_pipe.tail);
1159
1160         irq = platform_get_irq(pdev, 0);
1161         ret = devm_request_irq(dev, irq,
1162                                qcom_glink_intr,
1163                                IRQF_NO_SUSPEND | IRQF_SHARED,
1164                                "glink-rpm", glink);
1165         if (ret) {
1166                 dev_err(dev, "Failed to request IRQ\n");
1167                 return ret;
1168         }
1169
1170         glink->irq = irq;
1171
1172         ret = qcom_glink_send_version(glink);
1173         if (ret)
1174                 return ret;
1175
1176         platform_set_drvdata(pdev, glink);
1177
1178         return 0;
1179 }
1180
1181 static int glink_rpm_remove_device(struct device *dev, void *data)
1182 {
1183         device_unregister(dev);
1184
1185         return 0;
1186 }
1187
1188 static int glink_rpm_remove(struct platform_device *pdev)
1189 {
1190         struct qcom_glink *glink = platform_get_drvdata(pdev);
1191         struct glink_channel *channel;
1192         int cid;
1193         int ret;
1194
1195         disable_irq(glink->irq);
1196         cancel_work_sync(&glink->rx_work);
1197
1198         ret = device_for_each_child(glink->dev, NULL, glink_rpm_remove_device);
1199         if (ret)
1200                 dev_warn(glink->dev, "Can't remove GLINK devices: %d\n", ret);
1201
1202         /* Release any defunct local channels, waiting for close-ack */
1203         idr_for_each_entry(&glink->lcids, channel, cid)
1204                 kref_put(&channel->refcount, qcom_glink_channel_release);
1205
1206         idr_destroy(&glink->lcids);
1207         idr_destroy(&glink->rcids);
1208
1209         return 0;
1210 }
1211
1212 static const struct of_device_id glink_rpm_of_match[] = {
1213         { .compatible = "qcom,glink-rpm" },
1214         {}
1215 };
1216 MODULE_DEVICE_TABLE(of, glink_rpm_of_match);
1217
1218 static struct platform_driver glink_rpm_driver = {
1219         .probe = glink_rpm_probe,
1220         .remove = glink_rpm_remove,
1221         .driver = {
1222                 .name = "qcom_glink_rpm",
1223                 .of_match_table = glink_rpm_of_match,
1224         },
1225 };
1226
1227 static int __init glink_rpm_init(void)
1228 {
1229         return platform_driver_register(&glink_rpm_driver);
1230 }
1231 subsys_initcall(glink_rpm_init);
1232
1233 static void __exit glink_rpm_exit(void)
1234 {
1235         platform_driver_unregister(&glink_rpm_driver);
1236 }
1237 module_exit(glink_rpm_exit);
1238
1239 MODULE_AUTHOR("Bjorn Andersson <bjorn.andersson@linaro.org>");
1240 MODULE_DESCRIPTION("Qualcomm GLINK RPM driver");
1241 MODULE_LICENSE("GPL v2");