1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
|
/*
* Copyright (C) ST-Ericsson SA 2010,2011
*
* Author: Chris Blair <chris.blair@stericsson.com> for ST-Ericsson
*
* License terms: GNU General Public License (GPL) version 2
*
* U9500 <-> M6718 IPC protocol implementation using SPI:
* TX queue functionality.
*/
#include <linux/modem/m6718_spi/modem_driver.h>
#include "modem_util.h"
#define FRAME_LENGTH_ALIGN (4)
#define MAX_FRAME_COUNTER (256)
void ipc_queue_init(struct ipc_link_context *context)
{
spin_lock_init(&context->tx_q_update_lock);
atomic_set(&context->tx_q_count, 0);
context->tx_q_free = IPC_TX_QUEUE_MAX_SIZE;
INIT_LIST_HEAD(&context->tx_q);
context->tx_frame_counter = 0;
}
void ipc_queue_delete_frame(struct ipc_tx_queue *frame)
{
kfree(frame);
}
struct ipc_tx_queue *ipc_queue_new_frame(struct ipc_link_context *link_context,
u32 l2_length)
{
struct ipc_tx_queue *frame;
u32 padded_len = l2_length;
/* frame length padded to alignment boundary */
if (padded_len % FRAME_LENGTH_ALIGN)
padded_len += (FRAME_LENGTH_ALIGN -
(padded_len % FRAME_LENGTH_ALIGN));
dev_dbg(&link_context->sdev->dev,
"link %d: new frame: length %d, padded to %d\n",
link_context->link->id, l2_length, padded_len);
frame = kzalloc(sizeof(*frame) + padded_len, GFP_ATOMIC);
if (frame == NULL) {
dev_err(&link_context->sdev->dev,
"link %d error: failed to allocate frame\n",
link_context->link->id);
return NULL;
}
frame->actual_len = l2_length;
frame->len = padded_len;
frame->data = frame + 1;
return frame;
}
bool ipc_queue_is_empty(struct ipc_link_context *context)
{
unsigned long flags;
bool empty;
spin_lock_irqsave(&context->tx_q_update_lock, flags);
empty = list_empty(&context->tx_q);
spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
return empty;
}
int ipc_queue_push_frame(struct ipc_link_context *context, u8 channel,
u32 length, void *data)
{
u32 l2_hdr;
unsigned long flags;
struct ipc_tx_queue *frame;
int *tx_frame_counter = &context->tx_frame_counter;
int qcount;
/*
* Max queue size is only approximate so we allow it to go a few bytes
* over the limit
*/
if (context->tx_q_free < length) {
dev_dbg(&context->sdev->dev,
"link %d: tx queue full, wanted %d free %d\n",
context->link->id,
length,
context->tx_q_free);
return -EAGAIN;
}
frame = ipc_queue_new_frame(context, length + IPC_L2_HDR_SIZE);
if (frame == NULL)
return -ENOMEM;
/* create l2 header and copy to pdu buffer */
l2_hdr = ipc_util_make_l2_header(channel, length);
*(u32 *)frame->data = l2_hdr;
/* copy the l2 sdu into the pdu buffer after the header */
memcpy(frame->data + IPC_L2_HDR_SIZE, data, length);
spin_lock_irqsave(&context->tx_q_update_lock, flags);
frame->counter = *tx_frame_counter;
*tx_frame_counter = (*tx_frame_counter + 1) % MAX_FRAME_COUNTER;
list_add_tail(&frame->node, &context->tx_q);
qcount = atomic_add_return(1, &context->tx_q_count);
/* tx_q_free could go negative here */
context->tx_q_free -= frame->len;
#ifdef CONFIG_DEBUG_FS
context->tx_q_min = min(context->tx_q_free, context->tx_q_min);
#endif
spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
dev_dbg(&context->sdev->dev,
"link %d: push tx frame %d: %08x (ch %d len %d), "
"new count %d, new free %d\n",
context->link->id,
frame->counter,
l2_hdr,
ipc_util_get_l2_channel(l2_hdr),
ipc_util_get_l2_length(l2_hdr),
qcount,
context->tx_q_free);
return 0;
}
struct ipc_tx_queue *ipc_queue_get_frame(struct ipc_link_context *context)
{
unsigned long flags;
struct ipc_tx_queue *frame;
int qcount;
spin_lock_irqsave(&context->tx_q_update_lock, flags);
frame = list_first_entry(&context->tx_q, struct ipc_tx_queue, node);
list_del(&frame->node);
qcount = atomic_sub_return(1, &context->tx_q_count);
context->tx_q_free += frame->len;
spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
dev_dbg(&context->sdev->dev,
"link %d: get tx frame %d, new count %d, "
"new free %d\n",
context->link->id, frame->counter, qcount, context->tx_q_free);
return frame;
}
void ipc_queue_reset(struct ipc_link_context *context)
{
unsigned long flags;
struct ipc_tx_queue *frame;
int qcount;
spin_lock_irqsave(&context->tx_q_update_lock, flags);
qcount = atomic_read(&context->tx_q_count);
while (qcount != 0) {
frame = list_first_entry(&context->tx_q,
struct ipc_tx_queue, node);
list_del(&frame->node);
ipc_queue_delete_frame(frame);
qcount = atomic_sub_return(1, &context->tx_q_count);
}
spin_unlock_irqrestore(&context->tx_q_update_lock, flags);
}
|
