← Back to branch summary

~ubuntu-branches/ubuntu/trusty/acpid/trusty-proposed

« back to all changes in this revision

Viewing changes to ng/acpid/src/drivers/netlink.c

  • Committer: Bazaar Package Importer
  • Author(s): Michael Meskes
  • Date: 2009-11-17 14:50:01 UTC
  • mfrom: (2.1.12 squeeze)
  • Revision ID: james.westby@ubuntu.com-20091117145001-y5hevyg7gcg6uwjk
Tags: 1.0.10-4
Updated netlink patch to version 6.

Show diffs side-by-side

added added

removed removed

Lines of Context:
ng/acpid/src/drivers/netlink.c
 
1
 
 
2
#include <acpid/driver/netlink.h>
 
3
 
 
4
/**
 
5
 *      low-level netlink functions
 
6
 */
 
7
 
 
8
int netlink_open(struct acpi_channel_private *private, int protocol, int groups)
 
9
{
 
10
        private->fd = socket(AF_NETLINK, SOCK_RAW, protocol);
 
11
        if (private->fd < 0)
 
12
                return -1;
 
13
 
 
14
        memset(&private->local, 0, sizeof(private->local));
 
15
        private->local.nl_family = AF_NETLINK;
 
16
        private->local.nl_groups = groups;
 
17
 
 
18
        if (bind(private->fd, (struct sockaddr *)&private->local, sizeof(private->local)) < 0)
 
19
                goto fail;
 
20
 
 
21
        socklen_t len = sizeof(private->local);
 
22
        if (getsockname(private->fd, (struct sockaddr *)&private->local, &len) < 0)
 
23
                goto fail;
 
24
 
 
25
        private->seq = time(NULL);
 
26
 
 
27
        return 0;
 
28
 
 
29
fail:
 
30
        close(private->fd);
 
31
        return -1;
 
32
}
 
33
 
 
34
int netlink_send(struct acpi_channel_private *private, struct nlmsghdr *n, pid_t peer, int groups)
 
35
{
 
36
        struct sockaddr_nl nladdr = { .nl_family = AF_NETLINK, .nl_pid = peer, .nl_groups = groups };
 
37
 
 
38
        n->nlmsg_seq = ++private->seq;
 
39
        return sendto(private->fd, n, n->nlmsg_len, 0, (struct sockaddr *)&nladdr, sizeof(nladdr));
 
40
}
 
41
 
 
42
int netlink_recv(struct acpi_channel_private *private, struct nlmsghdr *n, pid_t *peer)
 
43
{
 
44
        struct sockaddr_nl nladdr;
 
45
        socklen_t len = sizeof(nladdr);
 
46
 
 
47
        int ret = recvfrom(private->fd, n, n->nlmsg_len, 0, (struct sockaddr *)&nladdr, &len);
 
48
        *peer = nladdr.nl_pid;
 
49
 
 
50
        return ret;
 
51
}
 
52
 
 
53
int netlink_wait(struct acpi_channel_private *private, struct nlmsghdr *n, pid_t peer)
 
54
{
 
55
        int len = n->nlmsg_len;
 
56
 
 
57
        for (;;) {
 
58
                pid_t sender;
 
59
 
 
60
                n->nlmsg_len = len;
 
61
                int ret = netlink_recv(private, n, &sender);
 
62
                if (ret < 0 || sender != peer)
 
63
                        continue;
 
64
 
 
65
                for (struct nlmsghdr * h = n; NLMSG_OK(h, ret); h = NLMSG_NEXT(h, ret)) {
 
66
                        if (h->nlmsg_pid != private->local.nl_pid || h->nlmsg_seq != private->seq)
 
67
                                continue;
 
68
 
 
69
                        if (h->nlmsg_type == NLMSG_ERROR)
 
70
                                return -1;
 
71
 
 
72
                        memcpy(n, h, h->nlmsg_len);
 
73
                        return 0;
 
74
                }
 
75
        }
 
76
}
 
77
 
 
78
void netlink_close(struct acpi_channel_private *private)
 
79
{
 
80
        close(private->fd);
 
81
}
 
82
 
 
83
/**
 
84
 *      netlink message attributes
 
85
 */
 
86
 
 
87
int netlink_attr_attach(struct nlmsghdr *n, int max, int type, const void *data, int alen)
 
88
{
 
89
        int len = NLA_LENGTH(alen);
 
90
        struct nlattr *nla;
 
91
 
 
92
        if (NLMSG_ALIGN(n->nlmsg_len) + NLA_ALIGN(len) > max)
 
93
                return -1;
 
94
 
 
95
        nla = NLMSG_TAIL(n);
 
96
        nla->nla_type = type;
 
97
        nla->nla_len = len;
 
98
        memcpy(NLA_DATA(nla), data, alen);
 
99
        n->nlmsg_len = NLMSG_ALIGN(n->nlmsg_len) + NLA_ALIGN(len);
 
100
        return 0;
 
101
}
 
102
 
 
103
int netlink_attr_parse(struct nlattr *table[], int max, struct nlattr *src, int len)
 
104
{
 
105
        memset(table, 0, sizeof(struct nlattr *) * (max + 1));
 
106
 
 
107
        while (NLA_OK(src, len)) {
 
108
                if (src->nla_type <= max)
 
109
                        table[src->nla_type] = src;
 
110
                src = NLA_NEXT(src, len);
 
111
        }
 
112
        return 0;
 
113
}
 
114
 
 
115
 
 
116
/**
 
117
 *      acpi netlink broadcast group discovery
 
118
 */
 
119
 
 
120
static const char *acpi_family_name = "acpi_event";
 
121
static const char *acpi_group_name = "acpi_mc_group";
 
122
 
 
123
static int acapi_netlink_group(struct nlattr *attr, int *group)
 
124
{
 
125
        if (attr == NULL)
 
126
                return -1;
 
127
 
 
128
        struct nlattr *attrs[CTRL_ATTR_MCAST_GRP_MAX + 1];
 
129
        netlink_attr_parse(attrs, CTRL_ATTR_MCAST_GRP_MAX, NLA_DATA(attr), attr->nla_len - NLA_HDRLEN);
 
130
 
 
131
        const char *name = NLA_DATA(attrs[CTRL_ATTR_MCAST_GRP_NAME]);
 
132
        if (strcmp(name, acpi_group_name))
 
133
                return -1;
 
134
 
 
135
        *group = *(__u32 *) (NLA_DATA(attrs[CTRL_ATTR_MCAST_GRP_ID]));
 
136
        return 0;
 
137
}
 
138
 
 
139
 
 
140
static int acpi_netlink_family(struct nlmsghdr *n, int *family, int *group)
 
141
{
 
142
        if (n->nlmsg_type != GENL_ID_CTRL)
 
143
                return -1;
 
144
        struct genlmsghdr *ghdr = NLMSG_DATA(n);
 
145
 
 
146
        if (ghdr->cmd != CTRL_CMD_NEWFAMILY)
 
147
                return -1;
 
148
 
 
149
        if (n->nlmsg_len < NLMSG_LENGTH(GENL_HDRLEN))
 
150
                return -1;
 
151
 
 
152
        struct nlattr *attrs[CTRL_ATTR_MAX + 1];
 
153
        netlink_attr_parse(attrs, CTRL_ATTR_MAX, NLMSG_DATA(n) + GENL_HDRLEN, NLMSG_PAYLOAD(n, GENL_HDRLEN));
 
154
 
 
155
        if (attrs[CTRL_ATTR_FAMILY_ID])
 
156
                *family = *(__u32 *) (NLA_DATA(attrs[CTRL_ATTR_FAMILY_ID]));
 
157
 
 
158
        if (attrs[CTRL_ATTR_MCAST_GROUPS]) {
 
159
                struct nlattr *attrs2[GENL_MAX_FAM_GRPS + 1];
 
160
                netlink_attr_parse(attrs2, GENL_MAX_FAM_GRPS, NLA_DATA(attrs[CTRL_ATTR_MCAST_GROUPS]), attrs[CTRL_ATTR_MCAST_GROUPS]->nla_len - NLA_HDRLEN);
 
161
 
 
162
                for (int i = 0; i < GENL_MAX_FAM_GRPS; i++) {
 
163
                        if (acapi_netlink_group(attrs2[i], group) == 0)
 
164
                                return 0;
 
165
                }
 
166
        }
 
167
 
 
168
        return 0;
 
169
}
 
170
 
 
171
int acpi_netlink_event(struct acpi_channel *channel, struct acpi_genl_event *events, int max)
 
172
{
 
173
        struct acpi_channel_private *private = channel->private;
 
174
 
 
175
        char buffer[256];
 
176
        struct nlmsghdr *n = (struct nlmsghdr *) &buffer;
 
177
        n->nlmsg_len = 256;
 
178
 
 
179
        pid_t sender;
 
180
        int ret = netlink_recv(private, n, &sender);
 
181
        if (ret < 0)
 
182
                return -1;
 
183
 
 
184
        int i = 0;
 
185
        for (struct nlmsghdr * h = n; NLMSG_OK(h, ret) && i < max; h = NLMSG_NEXT(h, ret), ++i) {
 
186
                if (h->nlmsg_type == NLMSG_ERROR)
 
187
                        return -1;
 
188
 
 
189
                struct nlattr *attrs[ACPI_GENL_ATTR_MAX + 1];
 
190
                netlink_attr_parse(attrs, ACPI_GENL_ATTR_MAX, NLMSG_DATA(h) + GENL_HDRLEN, NLMSG_PAYLOAD(h, GENL_HDRLEN));
 
191
 
 
192
                if (attrs[ACPI_GENL_ATTR_EVENT]) {
 
193
                        struct acpi_genl_event *event = NLA_DATA(attrs[ACPI_GENL_ATTR_EVENT]);
 
194
                        memcpy(events + i, event, sizeof(struct acpi_genl_event));
 
195
                }
 
196
        }
 
197
 
 
198
        return i;
 
199
}
 
200
 
 
201
 
 
202
 
 
203
/**
 
204
 *      acpi netlink channel
 
205
 */
 
206
 
 
207
static int setup(struct acpi_channel *channel, struct acpi_channel_descriptor *cds, unsigned long num)
 
208
{
 
209
        struct acpi_channel_private *private = (struct acpi_channel_private *) channel->private;
 
210
 
 
211
        acpi_channel_watch(channel, cds, private->fd, POLLIN);
 
212
 
 
213
        return 1;
 
214
}
 
215
 
 
216
static int handle(struct acpi_channel *channel, lua_State *L, int fd, int events)
 
217
{
 
218
        struct acpi_channel_private *private = channel->private;
 
219
 
 
220
        for (;;) {
 
221
                struct acpi_genl_event e;
 
222
                int ret = acpi_netlink_event(channel, &e, 1);
 
223
                if (ret <= 0)
 
224
                        return ret;
 
225
 
 
226
                printf("got event: %20s %15s %08x %08x\n", e.device_class, e.bus_id, e.type, e.data);
 
227
 
 
228
                //*strchr(e.bus_id, ':') = 0;
 
229
                acpi_channel_event(channel, L, e.bus_id, e.type, e.data);
 
230
        }
 
231
 
 
232
        return 0;
 
233
}
 
234
 
 
235
 
 
236
static int create(struct acpi_channel *channel, lua_State *L)
 
237
{
 
238
        static const struct acpi_channel_ops ops = { setup, handle };
 
239
        acpi_channel_register(channel, &ops);
 
240
 
 
241
        lua_pushinteger(L, 1);
 
242
        lua_gettable(L, -2);
 
243
        int ret = strcmp(lua_tostring(L, -1), "acpi");
 
244
        lua_pop(L, 1);
 
245
 
 
246
        if (ret)
 
247
                return -1;
 
248
 
 
249
        channel->private = malloc(sizeof(struct acpi_channel_private));
 
250
        if (channel->private == NULL)
 
251
                return -1;
 
252
 
 
253
        struct acpi_channel_private *private = channel->private;
 
254
        if (netlink_open(private, NETLINK_GENERIC, 0))
 
255
                return -1;
 
256
 
 
257
        char buffer[256];
 
258
        struct nlmsghdr *nlmsg = (struct nlmsghdr *)&buffer;
 
259
 
 
260
        nlmsg->nlmsg_len = NLMSG_LENGTH(GENL_HDRLEN);
 
261
        nlmsg->nlmsg_flags = NLM_F_REQUEST | NLM_F_ACK;
 
262
        nlmsg->nlmsg_type = GENL_ID_CTRL;
 
263
 
 
264
        struct genlmsghdr *ghdr = NLMSG_DATA(nlmsg);
 
265
        ghdr->cmd = CTRL_CMD_GETFAMILY;
 
266
 
 
267
        netlink_attr_attach(nlmsg, 128, CTRL_ATTR_FAMILY_NAME, acpi_family_name, strlen(acpi_family_name) + 1);
 
268
 
 
269
        if (netlink_send(private, nlmsg, 0, 0) < 0)
 
270
                return -1;
 
271
 
 
272
        nlmsg->nlmsg_len = 256;
 
273
        if (netlink_wait(private, nlmsg, 0) < 0)
 
274
                return -1;
 
275
 
 
276
        int family, group;
 
277
        if (acpi_netlink_family(nlmsg, &family, &group) < 0)
 
278
                return -1;
 
279
 
 
280
        netlink_close(private);
 
281
        if (netlink_open(private, NETLINK_GENERIC, group ? (1 << (group - 1)) : 0))
 
282
                return -1;
 
283
 
 
284
        int flags = fcntl(private->fd, F_GETFL, 0);
 
285
        fcntl(private->fd, F_SETFL, flags | O_NONBLOCK);
 
286
 
 
287
        return 0;
 
288
 
 
289
  ctrl_done:
 
290
        netlink_close(private);
 
291
        return -1;
 
292
}
 
293
 
 
294
static void destroy(struct acpi_channel *channel)
 
295
{
 
296
        struct acpi_channel_private *private = channel->private;
 
297
 
 
298
        close(private->fd);
 
299
        free(channel->private);
 
300
}
 
301
 
 
302
 
 
303
 
 
304
/*
 
305
 *      driver constuctor
 
306
 */
 
307
 
 
308
static __attribute__((constructor)) void constructor()
 
309
{
 
310
        static const struct acpi_driver driver = { "netlink", { create, destroy } };
 
311
        acpi_driver_register(&driver);
 
312
}
 
313